Revision: 7919
http://playerstage.svn.sourceforge.net/playerstage/?rev=7919&view=rev
Author: natepak
Date: 2009-06-27 01:46:18 +0000 (Sat, 27 Jun 2009)
Log Message:
-----------
Added a meter bar to the graphics 3d interface. Added more support for
getting/setting a robot's state via the simulation iface
Modified Paths:
--------------
code/gazebo/trunk/Media/materials/scripts/Gazebo.material
code/gazebo/trunk/examples/libgazebo/graphics3d/graphics3d.cc
code/gazebo/trunk/libgazebo/Graphics3dIface.cc
code/gazebo/trunk/libgazebo/SimIface.cc
code/gazebo/trunk/libgazebo/gazebo.h
code/gazebo/trunk/server/GraphicsIfaceHandler.cc
code/gazebo/trunk/server/GraphicsIfaceHandler.hh
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/Model.hh
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/main.cc
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/Body.hh
code/gazebo/trunk/webgazebo/Properties.cc
code/gazebo/trunk/webgazebo/WebGazebo.cc
code/gazebo/trunk/worlds/models/pioneer2dx.model
Modified: code/gazebo/trunk/Media/materials/scripts/Gazebo.material
===================================================================
--- code/gazebo/trunk/Media/materials/scripts/Gazebo.material 2009-06-27
00:15:41 UTC (rev 7918)
+++ code/gazebo/trunk/Media/materials/scripts/Gazebo.material 2009-06-27
01:46:18 UTC (rev 7919)
@@ -167,6 +167,7 @@
{
pass
{
+
ambient 1.000000 0.000000 0.000000 1.000000
diffuse 1.000000 0.000000 0.000000 1.000000
specular 0.200000 0.000000 0.000000 1.000000
@@ -333,6 +334,23 @@
}
}
+material Gazebo/QuestionMark
+{
+ technique
+ {
+ pass
+ {
+ depth_write off
+ scene_blend alpha_blend
+
+ texture_unit
+ {
+ texture questionMark.png
+ }
+ }
+ }
+}
+
material Gazebo/SmileyHappy
{
technique
Modified: code/gazebo/trunk/examples/libgazebo/graphics3d/graphics3d.cc
===================================================================
--- code/gazebo/trunk/examples/libgazebo/graphics3d/graphics3d.cc
2009-06-27 00:15:41 UTC (rev 7918)
+++ code/gazebo/trunk/examples/libgazebo/graphics3d/graphics3d.cc
2009-06-27 01:46:18 UTC (rev 7919)
@@ -216,9 +216,21 @@
sphereSize.z = 0.1;
// Draw a billboard
+ // The material used can be any of the predefined materials:
+ // Gazebo/SmileyHappy, Gazebo/SmileySad, Gazebo/SmileyPlain,
+ // Gazebo/SmileyDead, Gazebo/ExclamationPoint, Gazebo/QuestionMark
pioneerG3DIface->DrawBillboard("mybillboard", "Gazebo/SmileySad",
gazebo::Vec3(0.4,0.0,0.4), gazebo::Vec2(0.2, 0.2) );
+ gazebo::Color barClr;
+ barClr.r = 1.0;
+ barClr.g = 1.0;
+ barClr.b = 0.0;
+ barClr.a = 0.5;
+
+ float percent = 0.0;
+ int dir = 1.0;
+
// Update all the drawables
while (true)
{
@@ -227,6 +239,13 @@
UpdatePath();
UpdateText();
+ pioneerG3DIface->DrawMeterBar("mymeterbar", gazebo::Vec3(0.0, 0.0, 0.8),
+ gazebo::Vec2(0.5, 0.1),barClr, percent );
+
+ percent += dir * 0.01;
+ if (percent >= 1.0 || percent <= 0.0)
+ dir *= -1;
+
usleep(20000);
}
Modified: code/gazebo/trunk/libgazebo/Graphics3dIface.cc
===================================================================
--- code/gazebo/trunk/libgazebo/Graphics3dIface.cc 2009-06-27 00:15:41 UTC
(rev 7918)
+++ code/gazebo/trunk/libgazebo/Graphics3dIface.cc 2009-06-27 01:46:18 UTC
(rev 7919)
@@ -158,3 +158,35 @@
this->Unlock();
}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Draw a meter bar (progress bar)
+void Graphics3dIface::DrawMeterBar(const char *name, Vec3 pos, Vec2 size,
+ Color clr, float percent)
+{
+ this->Lock(1);
+ Graphics3dDrawData *cmd = &(this->data->commands[this->data->cmdCount++]);
+
+ cmd->drawMode = Graphics3dDrawData::METERBAR;
+
+ // Set the name of the graphics drawable
+ memset( cmd->name, 0, GAZEBO_GRAPHICS3D_MAX_NAME);
+ strcpy( cmd->name, name);
+
+ cmd->pose.pos.x = pos.x;
+ cmd->pose.pos.y = pos.y;
+ cmd->pose.pos.z = pos.z;
+
+ cmd->size.x = size.x;
+ cmd->size.y = size.y;
+ cmd->size.z = 0;
+
+ cmd->color.r = clr.r;
+ cmd->color.g = clr.g;
+ cmd->color.b = clr.b;
+ cmd->color.a = clr.a;
+
+ cmd->fltVar = percent;
+
+ this->Unlock();
+}
Modified: code/gazebo/trunk/libgazebo/SimIface.cc
===================================================================
--- code/gazebo/trunk/libgazebo/SimIface.cc 2009-06-27 00:15:41 UTC (rev
7918)
+++ code/gazebo/trunk/libgazebo/SimIface.cc 2009-06-27 01:46:18 UTC (rev
7919)
@@ -117,8 +117,6 @@
// Signal the callback function
this->goAckSignal();
}
-
- printf("Done with the thread\n");
}
////////////////////////////////////////////////////////////////////////////////
@@ -189,6 +187,7 @@
return true;
}
+
////////////////////////////////////////////////////////////////////////////////
/// Get the 2d pose of a model
bool SimulationIface::GetPose2d(const char *modelName, Pose &pose)
@@ -254,9 +253,9 @@
////////////////////////////////////////////////////////////////////////////////
/// Set the complete state of a model
-void SimulationIface::SetState(const char *modelName, const Pose &modelPose,
- const Vec3 &linearVel, const Vec3 &angularVel, const Vec3 &linearAccel,
- const Vec3 &angularAccel )
+void SimulationIface::SetState(const char *modelName, Pose &modelPose,
+ Vec3 &linearVel, Vec3 &angularVel, Vec3 &linearAccel,
+ Vec3 &angularAccel )
{
this->Lock(1);
this->data->responseCount = 0;
@@ -267,6 +266,20 @@
strncpy(request->modelName, modelName, 512);
request->modelName[511] = '\0';
+ if (isnan(linearVel.x)) linearVel.x = 0.0;
+ if (isnan(linearVel.y)) linearVel.y = 0.0;
+ if (isnan(linearVel.z)) linearVel.z = 0.0;
+ if (isnan(angularVel.x)) angularVel.x = 0.0;
+ if (isnan(angularVel.y)) angularVel.y = 0.0;
+ if (isnan(angularVel.z)) angularVel.z = 0.0;
+
+ if (isnan(linearAccel.x)) linearAccel.x = 0.0;
+ if (isnan(linearAccel.y)) linearAccel.y = 0.0;
+ if (isnan(linearAccel.z)) linearAccel.z = 0.0;
+ if (isnan(angularAccel.x)) angularAccel.x = 0.0;
+ if (isnan(angularAccel.y)) angularAccel.y = 0.0;
+ if (isnan(angularAccel.z)) angularAccel.z = 0.0;
+
request->modelPose = modelPose;
request->modelLinearVel = linearVel;
request->modelAngularVel = angularVel;
@@ -309,6 +322,38 @@
this->Unlock();
}
///////////////////////////////////////////////////////////////////////////////
+/// Get the complete state of a model
+bool SimulationIface::GetState(const char *modelName, Pose &modelPose,
+ Vec3 &linearVel, Vec3 &angularVel,
+ Vec3 &linearAccel, Vec3 &angularAccel )
+{
+ this->Lock(1);
+ this->data->responseCount = 0;
+ SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
+
+ request->type = gazebo::SimulationRequestData::GET_STATE;
+ memset(request->modelName, 0, 512);
+ strncpy(request->modelName, modelName, 512);
+ request->modelName[511] = '\0';
+
+ this->Unlock();
+
+ if (!this->WaitForResponse())
+ return false;
+
+ modelPose = this->data->responses[0].modelPose;
+
+ linearVel = this->data->responses[0].modelLinearVel;
+ angularVel = this->data->responses[0].modelAngularVel;
+
+ linearAccel = this->data->responses[0].modelLinearAccel;
+ angularAccel = this->data->responses[0].modelAngularAccel;
+
+ return true;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
// Wait for a post on the go ack semaphore
void SimulationIface::GoAckWait()
{
Modified: code/gazebo/trunk/libgazebo/gazebo.h
===================================================================
--- code/gazebo/trunk/libgazebo/gazebo.h 2009-06-27 00:15:41 UTC (rev
7918)
+++ code/gazebo/trunk/libgazebo/gazebo.h 2009-06-27 01:46:18 UTC (rev
7919)
@@ -426,6 +426,7 @@
SET_POSE3D,
SET_POSE2D,
SET_STATE,
+ GET_STATE,
GO,
GET_MODEL_TYPE,
GET_NUM_MODELS,
@@ -552,10 +553,14 @@
public: void SetPose2d(const char *modelName, float x, float y, float yaw);
/// \brief Set the complete state of a model
- public: void SetState(const char *modelName, const Pose &modelPose,
- const Vec3 &linearVel, const Vec3 &angularVel,
- const Vec3 &linearAccel, const Vec3 &angularAccel );
+ public: void SetState(const char *modelName, Pose &modelPose,
+ Vec3 &linearVel, Vec3 &angularVel,
+ Vec3 &linearAccel, Vec3 &angularAccel );
+ /// \brief Get the complete state of a model
+ public: bool GetState(const char *modelName, Pose &modelPose,
+ Vec3 &linearVel, Vec3 &angularVel,
+ Vec3 &linearAccel, Vec3 &angularAccel );
/// \brief Get the child interfaces of a model
public: void GetChildInterfaces(const char *modelName);
@@ -861,7 +866,7 @@
/// Type of drawing to perform
public: enum DrawMode { POINTS, LINES, LINE_STRIP, TRIANGLES,
TRIANGLE_STRIP,
TRIANGLE_FAN, PLANE, SPHERE, CUBE, CYLINDER, CONE,
- BILLBOARD, TEXT };
+ BILLBOARD, TEXT, METERBAR };
/// Drawing mode
public: DrawMode drawMode;
@@ -890,6 +895,8 @@
/// Size of the shape
public: Vec3 size;
+
+ public: float fltVar;
};
/// \brief Graphics3d interface data
@@ -936,7 +943,11 @@
public: void DrawText(const char *name, const char *text, Vec3 pos,
float fontSize);
+ /// \brief Draw a meter bar (progress bar)
+ public: void DrawMeterBar(const char *name, Vec3 pos, Vec2 size, Color clr,
+ float percent);
+
/// Pointer to the graphics3d data
public: Graphics3dData *data;
};
Modified: code/gazebo/trunk/server/GraphicsIfaceHandler.cc
===================================================================
--- code/gazebo/trunk/server/GraphicsIfaceHandler.cc 2009-06-27 00:15:41 UTC
(rev 7918)
+++ code/gazebo/trunk/server/GraphicsIfaceHandler.cc 2009-06-27 01:46:18 UTC
(rev 7919)
@@ -137,6 +137,10 @@
this->DrawText(vis, &this->threeDIface->data->commands[i] );
break;
+ case Graphics3dDrawData::METERBAR:
+ this->DrawMeterBar(vis, &this->threeDIface->data->commands[i] );
+ break;
+
default:
gzerr(0) << "Unknown draw mode["
<< this->threeDIface->data->commands[i].drawMode << "\n";
@@ -285,6 +289,7 @@
data->pose.pos.z) );
break;
}
+
case Graphics3dDrawData::BILLBOARD:
{
bool attached = false;
@@ -370,3 +375,162 @@
vis->AttachObject( msg );
}
+
+////////////////////////////////////////////////////////////////////////////////
+// Helper function used to draw a progress bar
+void GraphicsIfaceHandler::DrawMeterBar(OgreVisual *vis,
+ Graphics3dDrawData *data )
+{
+ bool attached = false;
+ Ogre::BillboardSet *bset = NULL;
+ std::ostringstream bname;
+ Ogre::TexturePtr texture;
+ Ogre::MaterialPtr material;
+
+ unsigned int width = 64;
+ unsigned int height = 64;
+ unsigned int rowWidth = width*4;
+
+ bname << "METERBAR_" << this->name;
+
+ if (vis->GetNumAttached() >0)
+ {
+ bset = (Ogre::BillboardSet *)vis->GetAttached(0);
+ bset->clear();
+ attached = true;
+ texture = Ogre::TextureManager::getSingleton().getByName(
+ bname.str()+"texture");
+ material = Ogre::MaterialManager::getSingleton().getByName(
+ bname.str()+"material");
+ }
+ else
+ {
+ bset = OgreAdaptor::Instance()->sceneMgr->createBillboardSet(
+ bname.str().c_str());
+
+ // Create the texture
+ texture = Ogre::TextureManager::getSingleton().createManual(
+ bname.str()+"texture",
+ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+ Ogre::TEX_TYPE_2D,
+ width, height,
+ 0, // number of mipmaps
+ Ogre::PF_BYTE_RGBA, // pixel format
+ Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE); // usage;
+ //should be TU_DYNAMIC_WRITE_ONLY_DISCARDABLE
+ //for textures updated very often (e.g. each
frame)
+
+ // Create a material using the texture
+ material = Ogre::MaterialManager::getSingleton().create(
+ bname.str()+"material",
+ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+
+ material->getTechnique(0)->getPass(0)->createTextureUnitState(
+ bname.str()+"texture");
+ material->getTechnique(0)->getPass(0)->setSceneBlending(
+ Ogre::SBT_TRANSPARENT_ALPHA);
+ }
+
+ Ogre::Billboard *billboard = bset->createBillboard(
+ Ogre::Vector3(data->pose.pos.x,
+ data->pose.pos.y,
+ data->pose.pos.z));
+
+ billboard->setDimensions(data->size.x, data->size.y);
+
+ // Get the pixel buffer
+ Ogre::HardwarePixelBufferSharedPtr pixelBuffer = texture->getBuffer();
+
+ // Lock the pixel buffer and get a pixel box
+ pixelBuffer->lock(Ogre::HardwareBuffer::HBL_NORMAL);
+
+ // for best performance use HBL_DISCARD!
+ const Ogre::PixelBox& pixelBox = pixelBuffer->getCurrentLock();
+
+ uint8* pDest = static_cast<uint8*>(pixelBox.data);
+
+ char red = data->color.b*255;
+ char green = data->color.g*255;
+ char blue = data->color.r*255;
+
+ // Fill in some pixel data.
+ for (size_t j = 0; j < height; j++)
+ {
+ for(size_t i = 0; i < width; i++)
+ {
+ pDest[j*rowWidth + i*4 + 0] = red;
+ pDest[j*rowWidth + i*4 + 1] = green;
+ pDest[j*rowWidth + i*4 + 2] = blue;
+
+ if (i < width * data->fltVar)
+ {
+ pDest[j*rowWidth + i*4 + 3] = 180;
+ }
+ else
+ pDest[j*rowWidth + i*4 + 3] = 0;
+ }
+ }
+
+
+ for (size_t j =0; j < width; j++)
+ {
+ // 2-pixel top border
+ pDest[j*4+0] = red;
+ pDest[j*4+1] = green;
+ pDest[j*4+2] = blue;
+ pDest[j*4+3] = 255;
+
+ pDest[rowWidth +j*4+0] = red;
+ pDest[rowWidth +j*4+1] = green;
+ pDest[rowWidth +j*4+2] = blue;
+ pDest[rowWidth +j*4+3] = 255;
+
+ // 2-pixel bottom border
+ pDest[(height-1)*rowWidth+j*4+0] = red;
+ pDest[(height-1)*rowWidth+j*4+1] = green;
+ pDest[(height-1)*rowWidth+j*4+2] = blue;
+ pDest[(height-1)*rowWidth+j*4+3] = 255;
+
+ pDest[(height-2)*rowWidth+j*4+0] = red;
+ pDest[(height-2)*rowWidth+j*4+1] = green;
+ pDest[(height-2)*rowWidth+j*4+2] = blue;
+ pDest[(height-2)*rowWidth+j*4+3] = 255;
+ }
+
+
+ for (size_t j =0; j < height; j++)
+ {
+ // 2-pixel left border
+ pDest[rowWidth*j+0] = red;
+ pDest[rowWidth*j+1] = green;
+ pDest[rowWidth*j+2] = blue;
+ pDest[rowWidth*j+3] = 255;
+
+ pDest[rowWidth*j+4] = red;
+ pDest[rowWidth*j+5] = green;
+ pDest[rowWidth*j+6] = blue;
+ pDest[rowWidth*j+7] = 255;
+
+ // 2-pixel right border
+ pDest[rowWidth*j + (width-1)*4+0] = red;
+ pDest[rowWidth*j + (width-1)*4+1] = green;
+ pDest[rowWidth*j + (width-1)*4+2] = blue;
+ pDest[rowWidth*j + (width-1)*4+3] = 255;
+
+ pDest[rowWidth*j + (width-2)*4+0] = red;
+ pDest[rowWidth*j + (width-2)*4+1] = green;
+ pDest[rowWidth*j + (width-2)*4+2] = blue;
+ pDest[rowWidth*j + (width-2)*4+3] = 255;
+ }
+
+ // Unlock the pixel buffer
+ pixelBuffer->unlock();
+
+ bset->setMaterialName(bname.str()+"material");
+
+ if (!attached)
+ {
+ vis->AttachObject(bset);
+ }
+
+}
Modified: code/gazebo/trunk/server/GraphicsIfaceHandler.hh
===================================================================
--- code/gazebo/trunk/server/GraphicsIfaceHandler.hh 2009-06-27 00:15:41 UTC
(rev 7918)
+++ code/gazebo/trunk/server/GraphicsIfaceHandler.hh 2009-06-27 01:46:18 UTC
(rev 7919)
@@ -64,6 +64,9 @@
/// \brief Helper funciton used to draw text
private: void DrawText(OgreVisual *vis, Graphics3dDrawData *data);
+ /// \brief Helper function used to draw a progress bar
+ private: void DrawMeterBar(OgreVisual *vis, Graphics3dDrawData *data );
+
private: std::string name;
private: std::map<std::string, OgreVisual* > visuals;
private: Graphics3dIface *threeDIface;
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2009-06-27 00:15:41 UTC (rev 7918)
+++ code/gazebo/trunk/server/Model.cc 2009-06-27 01:46:18 UTC (rev 7919)
@@ -692,7 +692,44 @@
body->SetAngularAccel( accel );
}
}
+
////////////////////////////////////////////////////////////////////////////////
+/// Get the linear velocity of the model
+Vector3 Model::GetLinearVel() const
+{
+ std::map<std::string, Body* >::const_iterator iter;
+ iter = this->bodies.begin();
+ return iter->second->GetLinearVel();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the angular velocity of the model
+Vector3 Model::GetAngularVel() const
+{
+ std::map<std::string, Body* >::const_iterator iter;
+ iter = this->bodies.begin();
+ return iter->second->GetAngularVel();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the linear acceleration of the model
+Vector3 Model::GetLinearAccel() const
+{
+ std::map<std::string, Body* >::const_iterator iter;
+ iter = this->bodies.begin();
+ return iter->second->GetLinearAccel();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the angular acceleration of the model
+Vector3 Model::GetAngularAccel() const
+{
+ std::map<std::string, Body* >::const_iterator iter;
+ iter = this->bodies.begin();
+ return iter->second->GetAngularAccel();
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Get the current pose
Pose3d Model::GetPose() const
{
Modified: code/gazebo/trunk/server/Model.hh
===================================================================
--- code/gazebo/trunk/server/Model.hh 2009-06-27 00:15:41 UTC (rev 7918)
+++ code/gazebo/trunk/server/Model.hh 2009-06-27 01:46:18 UTC (rev 7919)
@@ -131,6 +131,18 @@
/// \brief Set the angular acceleration of the model
public: void SetAngularAccel( const Vector3 &vel );
+
+ /// \brief Get the linear velocity of the model
+ public: Vector3 GetLinearVel() const;
+
+ /// \brief Get the angular velocity of the model
+ public: Vector3 GetAngularVel() const;
+
+ /// \brief Get the linear acceleration of the model
+ public: Vector3 GetLinearAccel() const;
+
+ /// \brief Get the angular acceleration of the model
+ public: Vector3 GetAngularAccel() const;
/// \brief Get the current pose
public: virtual Pose3d GetPose() const;
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2009-06-27 00:15:41 UTC (rev 7918)
+++ code/gazebo/trunk/server/World.cc 2009-06-27 01:46:18 UTC (rev 7919)
@@ -889,6 +889,59 @@
break;
}
+ case SimulationRequestData::GET_STATE:
+ {
+ Model *model = this->GetModelByName((char*)req->modelName);
+ if (model)
+ {
+ Pose3d pose;
+ Vector3 linearVel;
+ Vector3 angularVel;
+ Vector3 linearAccel;
+ Vector3 angularAccel;
+
+ pose = model->GetPose();
+
+ // Get the model's linear and angular velocity
+ linearVel = model->GetLinearVel();
+ angularVel = model->GetAngularVel();
+
+ // Get the model's linear and angular acceleration
+ linearAccel = model->GetLinearAccel();
+ angularAccel = model->GetAngularAccel();
+
+ response->modelPose.pos.x = pose.pos.x;
+ response->modelPose.pos.y = pose.pos.y;
+ response->modelPose.pos.z = pose.pos.z;
+
+ response->modelPose.roll = pose.rot.GetAsEuler().x;
+ response->modelPose.pitch = pose.rot.GetAsEuler().y;
+ response->modelPose.yaw = pose.rot.GetAsEuler().z;
+
+ response->modelLinearVel.x = linearVel.x;
+ response->modelLinearVel.y = linearVel.y;
+ response->modelLinearVel.z = linearVel.z;
+
+ response->modelAngularVel.x = angularVel.x;
+ response->modelAngularVel.y = angularVel.y;
+ response->modelAngularVel.z = angularVel.z;
+
+ response->modelLinearAccel.x = linearAccel.x;
+ response->modelLinearAccel.y = linearAccel.y;
+ response->modelLinearAccel.z = linearAccel.z;
+
+ response->modelAngularAccel.x = angularAccel.x;
+ response->modelAngularAccel.y = angularAccel.y;
+ response->modelAngularAccel.z = angularAccel.z;
+
+ response++;
+ this->simIface->data->responseCount += 1;
+ }
+ else
+ gzerr(0) << "Invalid model name[" << req->modelName << "] in
simulation interface Get State Request.\n";
+ break;
+ }
+
case SimulationRequestData::GET_POSE2D:
case SimulationRequestData::GET_POSE3D:
{
Modified: code/gazebo/trunk/server/main.cc
===================================================================
--- code/gazebo/trunk/server/main.cc 2009-06-27 00:15:41 UTC (rev 7918)
+++ code/gazebo/trunk/server/main.cc 2009-06-27 01:46:18 UTC (rev 7919)
@@ -138,7 +138,8 @@
fprintf(stderr, " -r : Run without a rendering engine\n");
fprintf(stderr, " -l <logfile> : Log to indicated file.\n");
fprintf(stderr, " -n : Do not do any time control\n");
- fprintf(stderr," -p : Run without physics engine\n");
+ fprintf(stderr, " -p : Run without physics engine\n");
+ fprintf(stderr, " -u : Start the simulation paused\n");
fprintf(stderr, " <worldfile> : load the the indicated world file\n");
return;
}
@@ -162,15 +163,14 @@
FILE *tmpFile;
int ch;
- char *flags = (char*)("l:hd:s:fgxt:nqper");
+ char *flags = (char*)("l:hd:s:fgxt:nqperu");
// Get letter options
while ((ch = getopt(argc, argv, flags)) != -1)
{
switch (ch)
{
- case 'e':
- std::cout << "Running in federation mode!\n";
+ case 'u':
optPaused = true;
break;
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-06-27 00:15:41 UTC (rev
7918)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-06-27 01:46:18 UTC (rev
7919)
@@ -420,6 +420,8 @@
this->physicsEngine->UnlockMutex();
}
+ this->linearAccel.Set(0,0,0);
+ this->angularAccel.Set(0,0,0);
}
////////////////////////////////////////////////////////////////////////////////
@@ -463,6 +465,13 @@
}
}*/
+
+ // Apply our linear accel
+ this->SetForce(this->linearAccel);
+
+ // Apply our angular accel
+ this->SetTorque(this->angularAccel);
+
#ifdef TIMING
double tmpT3 = Simulator::Instance()->GetWallTime();
std::cout << " Static SetPose dt (" << tmpT3-tmpT2 << ")" <<
std::endl;
@@ -1145,7 +1154,7 @@
/// Set the linear acceleration of the body
void Body::SetLinearAccel(const Vector3 &accel)
{
- this->SetForce( accel * this->GetMass());
+ this->linearAccel = accel;// * this->GetMass();
}
////////////////////////////////////////////////////////////////////////////////
@@ -1159,7 +1168,7 @@
/// Set the angular acceleration of the body
void Body::SetAngularAccel(const Vector3 &accel)
{
- this->SetTorque( accel * this->GetMass());
+ this->angularAccel = accel * this->GetMass();
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/physics/Body.hh
===================================================================
--- code/gazebo/trunk/server/physics/Body.hh 2009-06-27 00:15:41 UTC (rev
7918)
+++ code/gazebo/trunk/server/physics/Body.hh 2009-06-27 01:46:18 UTC (rev
7919)
@@ -251,6 +251,9 @@
private: OgreVisual *cgVisual;
+ private: Vector3 linearAccel;
+ private: Vector3 angularAccel;
+
/// User specified Mass Matrix
protected: ParamT<bool> *customMassMatrixP;
protected: ParamT<double> *cxP ;
Modified: code/gazebo/trunk/webgazebo/Properties.cc
===================================================================
--- code/gazebo/trunk/webgazebo/Properties.cc 2009-06-27 00:15:41 UTC (rev
7918)
+++ code/gazebo/trunk/webgazebo/Properties.cc 2009-06-27 01:46:18 UTC (rev
7919)
@@ -49,9 +49,12 @@
this->simIface->data->responseCount = 0;
gazebo::Pose pose;
+ gazebo::Vec3 linearVel, angularVel;
+ gazebo::Vec3 linearAccel, angularAccel;
// Ask Gazebo
- if (!this->simIface->GetPose3d(name.c_str(), pose))
+ if (!this->simIface->GetState(name.c_str(), pose, linearVel, angularVel,
+ linearAccel, angularAccel))
return false;
p.x = pose.pos.x;
@@ -61,6 +64,20 @@
p.p = pose.pitch;
p.a = pose.yaw;
+ v.x = linearVel.x;
+ v.y = linearVel.y;
+ v.z = linearVel.z;
+ v.r = angularVel.x;
+ v.p = angularVel.y;
+ v.y = angularVel.z;
+
+ a.x = linearAccel.x;
+ a.y = linearAccel.y;
+ a.z = linearAccel.z;
+ a.r = angularAccel.x;
+ a.p = angularAccel.y;
+ a.y = angularAccel.z;
+
return true;
}
Modified: code/gazebo/trunk/webgazebo/WebGazebo.cc
===================================================================
--- code/gazebo/trunk/webgazebo/WebGazebo.cc 2009-06-27 00:15:41 UTC (rev
7918)
+++ code/gazebo/trunk/webgazebo/WebGazebo.cc 2009-06-27 01:46:18 UTC (rev
7919)
@@ -128,6 +128,15 @@
return false;
int j = xmldata.find("\"", i+tag2.length());
xmldata.replace(i,j-i+1,std::string("name=\"" + name + "\""));
+
+
+ tag1 = "<model:physical";
+ tag2 = ">";
+ i = xmldata.find(tag1);
+ i = xmldata.find(tag2,i);
+
+ xmldata.insert(i+1,
"<collide>none</collide><enableGravity>false</enableGravity>" );
+
return true;
}
}
Modified: code/gazebo/trunk/worlds/models/pioneer2dx.model
===================================================================
--- code/gazebo/trunk/worlds/models/pioneer2dx.model 2009-06-27 00:15:41 UTC
(rev 7918)
+++ code/gazebo/trunk/worlds/models/pioneer2dx.model 2009-06-27 01:46:18 UTC
(rev 7919)
@@ -66,7 +66,7 @@
<geom:cylinder name="left_wheel_geom">
<size>0.075 0.05</size>
- <mass>0.1</mass>
+ <mass>0.5</mass>
<mu1>0.5</mu1>
<visual>
@@ -101,7 +101,7 @@
<geom:cylinder name="right_wheel_geom">
<size>0.075 0.05</size>
- <mass>0.1</mass>
+ <mass>0.5</mass>
<mu1>0.5</mu1>
<visual>
@@ -134,7 +134,7 @@
<rpy>0 0 0</rpy>
<geom:sphere name="castor_geom">
<size>0.04</size>
- <mass>0.1</mass>
+ <mass>0.5</mass>
<mu1>0.5</mu1>
<visual>
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
