Revision: 8781
http://playerstage.svn.sourceforge.net/playerstage/?rev=8781&view=rev
Author: natepak
Date: 2010-06-22 23:01:14 +0000 (Tue, 22 Jun 2010)
Log Message:
-----------
Added hooks for the new plugins
Modified Paths:
--------------
code/gazebo/trunk/libgazebo/Client.cc
code/gazebo/trunk/libgazebo/FactoryIface.cc
code/gazebo/trunk/libgazebo/Graphics3dIface.cc
code/gazebo/trunk/libgazebo/Iface.cc
code/gazebo/trunk/libgazebo/IfaceFactory.cc
code/gazebo/trunk/libgazebo/IfaceFactory.hh
code/gazebo/trunk/libgazebo/Server.cc
code/gazebo/trunk/libgazebo/SimIface.cc
code/gazebo/trunk/libgazebo/gz.h
Modified: code/gazebo/trunk/libgazebo/Client.cc
===================================================================
--- code/gazebo/trunk/libgazebo/Client.cc 2010-06-22 23:00:46 UTC (rev
8780)
+++ code/gazebo/trunk/libgazebo/Client.cc 2010-06-22 23:01:14 UTC (rev
8781)
@@ -43,7 +43,7 @@
#include "gz.h"
-using namespace gazebo;
+using namespace libgazebo;
////////////////////////////////////////////////////////////////////////////////
// Create a client object
@@ -85,7 +85,7 @@
void Client::ConnectWait(int serverId, int clientId)
{
bool simulationIfaceIsValid = false;
- gazebo::SimulationIface *simIface = new gazebo::SimulationIface();
+ libgazebo::SimulationIface *simIface = new libgazebo::SimulationIface();
while (!simulationIfaceIsValid)
{
@@ -157,14 +157,14 @@
{
// a gazebo process is still alive.
// it might however, still being booted up, so we need to check by
connecting
- // Connect to gazebo::SimulationIface and check for changing
realTime,
+ // Connect to libgazebo::SimulationIface and check for changing
realTime,
// if simulationIface->data->realTime is not changing, the server
might
// be stale leftovers from previous gazebo crash,
// disconnect and reconnect client
try
{
// Open the Simulation Interface
- gazebo::SimulationIface simulationIface;
+ libgazebo::SimulationIface simulationIface;
simulationIface.Open(this,"default");
// grab a t0
simulationIface.Lock(1);
Modified: code/gazebo/trunk/libgazebo/FactoryIface.cc
===================================================================
--- code/gazebo/trunk/libgazebo/FactoryIface.cc 2010-06-22 23:00:46 UTC (rev
8780)
+++ code/gazebo/trunk/libgazebo/FactoryIface.cc 2010-06-22 23:01:14 UTC (rev
8781)
@@ -2,7 +2,7 @@
#include "gz.h"
-using namespace gazebo;
+using namespace libgazebo;
/// Delete a model by name
bool FactoryIface::DeleteEntity(const std::string &model_name)
Modified: code/gazebo/trunk/libgazebo/Graphics3dIface.cc
===================================================================
--- code/gazebo/trunk/libgazebo/Graphics3dIface.cc 2010-06-22 23:00:46 UTC
(rev 8780)
+++ code/gazebo/trunk/libgazebo/Graphics3dIface.cc 2010-06-22 23:01:14 UTC
(rev 8781)
@@ -2,7 +2,7 @@
#include <iostream>
-using namespace gazebo;
+using namespace libgazebo;
////////////////////////////////////////////////////////////////////////////////
/// Constructor
@@ -190,3 +190,21 @@
this->Unlock();
}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Draw a ribbon trail following an entity
+void Graphics3dIface::DrawRibbonTrail(const std::string &name)
+{
+ this->Lock(1);
+ Graphics3dDrawData *cmd = &(this->data->commands[this->data->cmdCount++]);
+
+ cmd->drawMode = Graphics3dDrawData::RIBBONTRAIL;
+
+ // Set the name of the graphics drawable
+ memset( cmd->name, 0, GAZEBO_GRAPHICS3D_MAX_NAME);
+ strcpy( cmd->name, name.c_str());
+
+ cmd->intVar = 1;
+
+ this->Unlock();
+}
Modified: code/gazebo/trunk/libgazebo/Iface.cc
===================================================================
--- code/gazebo/trunk/libgazebo/Iface.cc 2010-06-22 23:00:46 UTC (rev
8780)
+++ code/gazebo/trunk/libgazebo/Iface.cc 2010-06-22 23:01:14 UTC (rev
8781)
@@ -43,7 +43,7 @@
#include "gz.h"
-using namespace gazebo;
+using namespace libgazebo;
GZ_REGISTER_IFACE("simulation", SimulationIface);
GZ_REGISTER_IFACE("position", PositionIface);
Modified: code/gazebo/trunk/libgazebo/IfaceFactory.cc
===================================================================
--- code/gazebo/trunk/libgazebo/IfaceFactory.cc 2010-06-22 23:00:46 UTC (rev
8780)
+++ code/gazebo/trunk/libgazebo/IfaceFactory.cc 2010-06-22 23:01:14 UTC (rev
8781)
@@ -31,7 +31,7 @@
#include "gz.h"
#include "IfaceFactory.hh"
-using namespace gazebo;
+using namespace libgazebo;
std::map<std::string, IfaceFactoryFn> IfaceFactory::ifaces;
Modified: code/gazebo/trunk/libgazebo/IfaceFactory.hh
===================================================================
--- code/gazebo/trunk/libgazebo/IfaceFactory.hh 2010-06-22 23:00:46 UTC (rev
8780)
+++ code/gazebo/trunk/libgazebo/IfaceFactory.hh 2010-06-22 23:01:14 UTC (rev
8781)
@@ -39,7 +39,7 @@
}
};
-namespace gazebo
+namespace libgazebo
{
class Iface;
Modified: code/gazebo/trunk/libgazebo/Server.cc
===================================================================
--- code/gazebo/trunk/libgazebo/Server.cc 2010-06-22 23:00:46 UTC (rev
8780)
+++ code/gazebo/trunk/libgazebo/Server.cc 2010-06-22 23:01:14 UTC (rev
8781)
@@ -42,7 +42,7 @@
#include "gz.h"
-using namespace gazebo;
+using namespace libgazebo;
union semun
{
Modified: code/gazebo/trunk/libgazebo/SimIface.cc
===================================================================
--- code/gazebo/trunk/libgazebo/SimIface.cc 2010-06-22 23:00:46 UTC (rev
8780)
+++ code/gazebo/trunk/libgazebo/SimIface.cc 2010-06-22 23:01:14 UTC (rev
8781)
@@ -12,7 +12,7 @@
#include "gz.h"
-using namespace gazebo;
+using namespace libgazebo;
union semun
@@ -294,7 +294,7 @@
this->data->responseCount = 0;
SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
- request->type = gazebo::SimulationRequestData::SET_POSE2D;
+ request->type = libgazebo::SimulationRequestData::SET_POSE2D;
memset(request->name, 0, 512);
strncpy(request->name, name.c_str(), 512);
@@ -317,7 +317,7 @@
this->data->responseCount = 0;
SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
- request->type = gazebo::SimulationRequestData::SET_STATE;
+ request->type = libgazebo::SimulationRequestData::SET_STATE;
memset(request->name, 0, 512);
strncpy(request->name, name.c_str(), 512);
request->name[511] = '\0';
@@ -354,7 +354,7 @@
this->Lock(1);
SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
- request->type = gazebo::SimulationRequestData::GET_MODEL_INTERFACES;
+ request->type = libgazebo::SimulationRequestData::GET_MODEL_INTERFACES;
memset(request->name, 0, 512);
strncpy(request->name, name.c_str(), 512);
@@ -370,7 +370,7 @@
this->Lock(1);
SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
- request->type = gazebo::SimulationRequestData::GET_INTERFACE_TYPE;
+ request->type = libgazebo::SimulationRequestData::GET_INTERFACE_TYPE;
memset(request->name, 0, 512);
strncpy(request->name, name.c_str(), 512);
@@ -389,7 +389,7 @@
this->data->responseCount = 0;
SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
- request->type = gazebo::SimulationRequestData::GET_STATE;
+ request->type = libgazebo::SimulationRequestData::GET_STATE;
memset(request->name, 0, 512);
strncpy(request->name, name.c_str(), 512);
request->name[511] = '\0';
@@ -418,7 +418,7 @@
this->data->responseCount = 0;
SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
- request->type = gazebo::SimulationRequestData::SET_LINEAR_VEL;
+ request->type = libgazebo::SimulationRequestData::SET_LINEAR_VEL;
memset(request->name, 0, 512);
strncpy(request->name, modelName.c_str(), 512);
request->name[511] = '\0';
@@ -440,7 +440,7 @@
this->data->responseCount = 0;
SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
- request->type = gazebo::SimulationRequestData::SET_LINEAR_ACCEL;
+ request->type = libgazebo::SimulationRequestData::SET_LINEAR_ACCEL;
memset(request->name, 0, 512);
strncpy(request->name, modelName.c_str(), 512);
request->name[511] = '\0';
@@ -462,7 +462,7 @@
this->data->responseCount = 0;
SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
- request->type = gazebo::SimulationRequestData::SET_ANGULAR_VEL;
+ request->type = libgazebo::SimulationRequestData::SET_ANGULAR_VEL;
memset(request->name, 0, 512);
strncpy(request->name, modelName.c_str(), 512);
request->name[511] = '\0';
@@ -484,7 +484,7 @@
this->data->responseCount = 0;
SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
- request->type = gazebo::SimulationRequestData::SET_ANGULAR_ACCEL;
+ request->type = libgazebo::SimulationRequestData::SET_ANGULAR_ACCEL;
memset(request->name, 0, 512);
strncpy(request->name, modelName.c_str(), 512);
request->name[511] = '\0';
@@ -499,6 +499,50 @@
}
////////////////////////////////////////////////////////////////////////////////
+/// Apply a force to an entity
+void SimulationIface::ApplyForce(const std::string &entityName, Vec3 force)
+{
+ this->Lock(1);
+ this->data->responseCount = 0;
+ SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
+
+ request->type = libgazebo::SimulationRequestData::APPLY_FORCE;
+ memset(request->name, 0, 512);
+ strncpy(request->name, entityName.c_str(), 512);
+ request->name[511] = '\0';
+
+ if (isnan(force.x)) force.x = 0.0;
+ if (isnan(force.y)) force.y = 0.0;
+ if (isnan(force.z)) force.z = 0.0;
+
+ request->vec3Value = force;
+
+ this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Apply a torque to an entity
+void SimulationIface::ApplyTorque(const std::string &entityName, Vec3 torque)
+{
+ this->Lock(1);
+ this->data->responseCount = 0;
+ SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
+
+ request->type = libgazebo::SimulationRequestData::APPLY_TORQUE;
+ memset(request->name, 0, 512);
+ strncpy(request->name, entityName.c_str(), 512);
+ request->name[511] = '\0';
+
+ if (isnan(torque.x)) torque.x = 0.0;
+ if (isnan(torque.y)) torque.y = 0.0;
+ if (isnan(torque.z)) torque.z = 0.0;
+
+ request->vec3Value = torque;
+
+ this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Wait for a post on the go ack semaphore
void SimulationIface::GoAckWait()
{
@@ -851,3 +895,196 @@
return true;
}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Start logging entity information
+void SimulationIface::StartLogEntity(const std::string &entityName,
+ const std::string &filename)
+{
+ this->Lock(1);
+
+ this->data->responseCount = 0;
+ SimulationRequestData *request;
+
+ request = &(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::START_LOG;
+
+ memset(request->name, 0, 512);
+ strncpy(request->name, entityName.c_str(), 512);
+ request->name[511] = '\0';
+
+ memset(request->strValue, 0, 512);
+ strncpy(request->strValue, filename.c_str(), 512);
+ request->strValue[511] = '\0';
+
+ this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Stop logging entity information
+void SimulationIface::StopLogEntity(const std::string &entityName)
+{
+ this->Lock(1);
+
+ this->data->responseCount = 0;
+ SimulationRequestData *request;
+
+ request = &(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::STOP_LOG;
+
+ memset(request->name, 0, 512);
+ strncpy(request->name, entityName.c_str(), 512);
+ request->name[511] = '\0';
+
+ this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set the step time
+void SimulationIface::SetStepTime(double time)
+{
+ this->Lock(1);
+
+ this->data->responseCount = 0;
+ SimulationRequestData *request;
+
+ request = &(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::SET_STEP_TIME;
+ request->dblValue = time;
+ this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set the step iteraction
+void SimulationIface::SetStepIterations(unsigned int iters)
+{
+ this->Lock(1);
+
+ this->data->responseCount = 0;
+ SimulationRequestData *request;
+
+ request = &(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::SET_STEP_ITERS;
+ request->uintValue = iters;
+
+ this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set the step type
+void SimulationIface::SetStepType(const std::string &type)
+{
+ this->Lock(1);
+
+ this->data->responseCount = 0;
+ SimulationRequestData *request;
+
+ request = &(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::SET_STEP_TYPE;
+ memset(request->strValue, 0, 512);
+ strncpy(request->strValue, type.c_str(), 512);
+ request->strValue[511] = '\0';
+
+ this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the step type
+std::string SimulationIface::GetStepType()
+{
+ this->Lock(1);
+ this->data->responseCount = 0;
+
+ SimulationRequestData *request;
+ request = &(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::GET_STEP_TYPE;
+
+ this->Unlock();
+ if (!this->WaitForResponse())
+ return false;
+
+ assert(this->data->responseCount == 1);
+ return data->responses[0].strValue;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// The number of plugins in the simulation
+bool SimulationIface::GetPluginCount(unsigned int &count)
+{
+ this->Lock(1);
+ this->data->responseCount = 0;
+ SimulationRequestData *request;
+ request = &(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::GET_PLUGIN_COUNT;
+ this->Unlock();
+
+ if (!this->WaitForResponse())
+ return false;
+
+ assert(this->data->responseCount == 1);
+ count = data->responses[0].uintValue;
+ return true;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the name of a plugin
+bool SimulationIface::GetPluginName(unsigned int i, std::string &name)
+{
+ this->Lock(1);
+
+ this->data->responseCount = 0;
+ SimulationRequestData *request;
+
+ request = &(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::GET_PLUGIN_NAME;
+ request->uintValue = i;
+
+ this->Unlock();
+ if (!this->WaitForResponse())
+ return false;
+
+ assert(this->data->responseCount == 1);
+ name = data->responses[0].strValue;
+ return true;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Add a plugin
+void SimulationIface::AddPlugin(const std::string &filename, const std::string
&handle)
+{
+ this->Lock(1);
+
+ this->data->responseCount = 0;
+ SimulationRequestData *request;
+
+ request = &(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::ADD_PLUGIN;
+
+ memset(request->strValue, 0, 512);
+ strncpy(request->strValue, filename.c_str(), 512);
+ request->strValue[511] = '\0';
+
+ memset(request->name, 0, 512);
+ strncpy(request->name, handle.c_str(), 512);
+ request->name[511] = '\0';
+
+ this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Remove a plugin
+void SimulationIface::RemovePlugin(const std::string &name)
+{
+ this->Lock(1);
+
+ this->data->responseCount = 0;
+ SimulationRequestData *request;
+
+ request = &(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::REMOVE_PLUGIN;
+ memset(request->strValue, 0, 512);
+ strncpy(request->strValue, name.c_str(), 512);
+ request->strValue[511] = '\0';
+
+ this->Unlock();
+}
Modified: code/gazebo/trunk/libgazebo/gz.h
===================================================================
--- code/gazebo/trunk/libgazebo/gz.h 2010-06-22 23:00:46 UTC (rev 8780)
+++ code/gazebo/trunk/libgazebo/gz.h 2010-06-22 23:01:14 UTC (rev 8781)
@@ -41,7 +41,7 @@
#include "IfaceFactory.hh"
-namespace gazebo
+namespace libgazebo
{
@@ -447,6 +447,18 @@
GET_MODEL_INTERFACES, // for getting interfaces as well
as the models which are ancestors of interfaces
GET_INTERFACE_TYPE, // if the model is not an
interface 'unknown' is returned
SET_ENTITY_PARAM_VALUE,
+ START_LOG,
+ STOP_LOG,
+ SET_STEP_TIME,
+ SET_STEP_ITERS,
+ SET_STEP_TYPE,
+ GET_STEP_TYPE,
+ APPLY_FORCE,
+ APPLY_TORQUE,
+ GET_PLUGIN_COUNT,
+ GET_PLUGIN_NAME,
+ ADD_PLUGIN,
+ REMOVE_PLUGIN
};
public: Type type;
@@ -455,6 +467,7 @@
public: char strValue1[512];
public: Vec3 vec3Value;
public: unsigned int uintValue;
+ public: double dblValue;
public: Pose modelPose;
public: Vec3 modelLinearVel;
@@ -465,7 +478,6 @@
public: char childInterfaces[GAZEBO_MAX_NUMBER_OF_CHILDREN][512];
public: int nChildInterfaces;
//public: char modelType[512];
-
};
/// \brief Simulation interface data
@@ -482,6 +494,9 @@
/// Elapsed real time since start of simulation (from system clock)
public: double realTime;
+ /// Size of a simulation step
+ public: double stepTime;
+
/// state of the simulation : 0 paused, 1 running -1 not_started/exiting
public: int state;
@@ -598,6 +613,12 @@
/// \brief Set the angular acceleration
public: void SetAngularAccel(const std::string &modelName,Vec3 accel);
+ /// \brief Apply a force to an entity
+ public: void ApplyForce(const std::string &entityName, Vec3 force);
+
+ /// \brief Apply a torque to an entity
+ public: void ApplyTorque(const std::string &entityName, Vec3 torque);
+
/// \brief Get the child interfaces of a model
public: void GetChildInterfaces(const std::string &modelName);
@@ -650,6 +671,37 @@
public: void GoAckWait();
public: void GoAckPost();
+ /// \brief Start logging entity information
+ public: void StartLogEntity(const std::string &entityName,
+ const std::string &filename);
+
+ /// \brief Stop logging entity information
+ public: void StopLogEntity(const std::string &entityName);
+
+ /// \brief Set the step time
+ public: void SetStepTime(double time);
+
+ /// \brief Set the step iteraction
+ public: void SetStepIterations(unsigned int iters);
+
+ /// \brief Set the step type
+ public: void SetStepType(const std::string &type);
+
+ /// \brief Get the step type
+ public: std::string GetStepType();
+
+ /// \brief Get the number of plugins
+ public: bool GetPluginCount(unsigned int &count);
+
+ /// \brief Get the name of a plugin
+ public: bool GetPluginName(unsigned int i, std::string &name);
+
+ /// \brief Add a plugin
+ public: void AddPlugin(const std::string &filename,const std::string
&handle);
+
+ /// \brief Remove a plugin
+ public: void RemovePlugin(const std::string &name);
+
private: void BlockThread();
/// \brief Wait for a return message
@@ -925,7 +977,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, METERBAR };
+ BILLBOARD, TEXT, METERBAR, RIBBONTRAIL };
/// Drawing mode
public: DrawMode drawMode;
@@ -956,6 +1008,8 @@
public: Vec3 size;
public: float fltVar;
+
+ public: int intVar;
};
/// \brief Graphics3d interface data
@@ -1006,6 +1060,8 @@
public: void DrawMeterBar(const char *name, Vec3 pos, Vec2 size, Color clr,
float percent);
+ /// \brief Draw a ribbon trail following an entity
+ public: void DrawRibbonTrail(const std::string &name);
/// Pointer to the graphics3d data
public: Graphics3dData *data;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
ThinkGeek and WIRED's GeekDad team up for the Ultimate
GeekDad Father's Day Giveaway. ONE MASSIVE PRIZE to the
lucky parental unit. See the prize list and enter to win:
http://p.sf.net/sfu/thinkgeek-promo
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
