Revision: 8615
http://playerstage.svn.sourceforge.net/playerstage/?rev=8615&view=rev
Author: natepak
Date: 2010-04-23 23:57:16 +0000 (Fri, 23 Apr 2010)
Log Message:
-----------
Updates to gazebomodel tool
Modified Paths:
--------------
code/gazebo/trunk/libgazebo/SimIface.cc
code/gazebo/trunk/libgazebo/gz.h
code/gazebo/trunk/server/Common.cc
code/gazebo/trunk/server/Common.hh
code/gazebo/trunk/server/Factory.cc
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/World.hh
code/gazebo/trunk/server/gui/MainMenu.cc
code/gazebo/trunk/server/gui/Sidebar.cc
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/ode/ODEGeom.cc
code/gazebo/trunk/tools/CMakeLists.txt
code/gazebo/trunk/tools/gazebomodel.cc
Modified: code/gazebo/trunk/libgazebo/SimIface.cc
===================================================================
--- code/gazebo/trunk/libgazebo/SimIface.cc 2010-04-19 22:39:25 UTC (rev
8614)
+++ code/gazebo/trunk/libgazebo/SimIface.cc 2010-04-23 23:57:16 UTC (rev
8615)
@@ -14,6 +14,17 @@
using namespace gazebo;
+
+union semun
+{
+ int val; /* Value for SETVAL */
+ struct semid_ds *buf; /* Buffer for IPC_STAT, IPC_SET */
+ unsigned short *array; /* Array for GETALL, SETALL */
+ struct seminfo *__buf;
+} arg;
+
+
+
////////////////////////////////////////////////////////////////////////////////
/// Create an interface
SimulationIface::SimulationIface()
@@ -63,14 +74,6 @@
/// Create a simulation interface
void SimulationIface::Create(Server *server, std::string id)
{
- union semun
- {
- int val; /* Value for SETVAL */
- struct semid_ds *buf; /* Buffer for IPC_STAT, IPC_SET */
- unsigned short *array; /* Array for GETALL, SETALL */
- struct seminfo *__buf;
- } arg;
-
Iface::Create(server,id);
this->data = (SimulationData*)((char*)this->mMap+sizeof(SimulationIface));
@@ -90,6 +93,20 @@
printf("Semctl failed\n");
}
+//////////////////////////////////////////////////////////////////////////////
+// Destroy the interface (server)
+void SimulationIface::Destroy()
+{
+ if (this->data && semctl(this->data->semId, 0, IPC_RMID, arg) < 0)
+ {
+ std::ostringstream stream;
+ stream << "failed to deallocate semaphore [" << strerror(errno) << "]";
+ throw(stream.str());
+ }
+
+ Iface::Destroy();
+}
+
////////////////////////////////////////////////////////////////////////////////
/// Open a simulation interface
void SimulationIface::Open(Client *client, std::string id)
@@ -591,3 +608,91 @@
return true;
}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the number of parameters for an entity
+bool SimulationIface::GetEntityParamCount(const std::string &entityName,
+ unsigned int &num)
+{
+ this->Lock(1);
+ this->data->responseCount = 0;
+
+ SimulationRequestData *request;
+
+ request = &(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::GET_ENTITY_PARAM_COUNT;
+
+ memset(request->name, 0, 512);
+ strncpy(request->name, entityName.c_str(), 512);
+ request->name[511] = '\0';
+
+ this->Unlock();
+
+ if (!this->WaitForResponse())
+ return false;
+
+ assert(this->data->responseCount == 1);
+ num = data->responses[0].uintValue;
+
+ return true;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get a param key for an entity
+bool SimulationIface::GetEntityParamKey(const std::string &entityName,
+ unsigned int paramIndex, std::string ¶mKey )
+{
+ this->Lock(1);
+
+ this->data->responseCount = 0;
+ SimulationRequestData *request;
+
+ request = &(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::GET_ENTITY_PARAM_KEY;
+
+ memset(request->name, 0, 512);
+ strncpy(request->name, entityName.c_str(), 512);
+ request->name[511] = '\0';
+
+ request->uintValue = paramIndex;
+
+ this->Unlock();
+
+ if (!this->WaitForResponse())
+ return false;
+
+ assert(this->data->responseCount == 1);
+ paramKey = data->responses[0].strValue;
+
+ return true;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get a param value from an entity
+bool SimulationIface::GetEntityParamValue(const std::string &entityName,
+ unsigned int paramIndex, std::string ¶mValue )
+{
+ this->Lock(1);
+
+ this->data->responseCount = 0;
+ SimulationRequestData *request;
+
+ request = &(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::GET_ENTITY_PARAM_VALUE;
+
+ memset(request->name, 0, 512);
+ strncpy(request->name, entityName.c_str(), 512);
+ request->name[511] = '\0';
+
+ request->uintValue = paramIndex;
+
+ this->Unlock();
+
+ if (!this->WaitForResponse())
+ return false;
+
+ assert(this->data->responseCount == 1);
+ paramValue = data->responses[0].strValue;
+
+ return true;
+}
Modified: code/gazebo/trunk/libgazebo/gz.h
===================================================================
--- code/gazebo/trunk/libgazebo/gz.h 2010-04-19 22:39:25 UTC (rev 8614)
+++ code/gazebo/trunk/libgazebo/gz.h 2010-04-23 23:57:16 UTC (rev 8615)
@@ -317,7 +317,7 @@
int parentModelId);
/// \brief Destroy the interface (server)
- public: void Destroy();
+ public: virtual void Destroy();
/// \brief Open an existing interface
/// \param client Pointer to the client
@@ -429,6 +429,9 @@
GET_STATE,
GO,
GET_ENTITY_TYPE,
+ GET_ENTITY_PARAM_COUNT,
+ GET_ENTITY_PARAM_KEY,
+ GET_ENTITY_PARAM_VALUE,
GET_MODEL_TYPE,
GET_NUM_MODELS,
GET_NUM_CHILDREN,
@@ -502,6 +505,10 @@
/// \brief id String id
public: virtual void Create(Server *server, std::string id);
+ /// \brief Destroy the interface (server)
+ public: virtual void Destroy();
+
+
/// \brief Open a simulation interface
/// \param client Pointer to the client
/// \param id String name of the client
@@ -599,6 +606,19 @@
public: bool GetModelFiducialID(const std::string &modelName,
unsigned int &id);
+ /// \brief Get the number of parameters for an entity
+ public: bool GetEntityParamCount(const std::string &entityName,
+ unsigned int &num);
+
+ /// \brief Get a param key for an entity
+ public: bool GetEntityParamKey(const std::string &entityName,
+ unsigned int paramIndex, std::string ¶mKey );
+
+ /// \brief Get a param value from an entity
+ public: bool GetEntityParamValue(const std::string &entityName,
+ unsigned int paramIndex, std::string ¶mValue );
+
+
public: void GoAckWait();
public: void GoAckPost();
Modified: code/gazebo/trunk/server/Common.cc
===================================================================
--- code/gazebo/trunk/server/Common.cc 2010-04-19 22:39:25 UTC (rev 8614)
+++ code/gazebo/trunk/server/Common.cc 2010-04-23 23:57:16 UTC (rev 8615)
@@ -67,13 +67,24 @@
}
////////////////////////////////////////////////////////////////////////////////
-/// Get the parameters
-std::vector<Param*> *Common::GetParams()
+/// Get the count of the parameters
+unsigned int Common::GetParamCount() const
{
- return &this->parameters;
+ return this->parameters.size();
}
////////////////////////////////////////////////////////////////////////////////
+/// Get a param by index
+Param *Common::GetParam(unsigned int index) const
+{
+ if (index < this->parameters.size())
+ return this->parameters[index];
+ else
+ gzerr(0) << "Invalid index[" << index << "]\n";
+ return NULL;
+}
+
+////////////////////////////////////////////////////////////////////////////////
/// Get a parameter by name
Param *Common::GetParam(const std::string &key) const
{
Modified: code/gazebo/trunk/server/Common.hh
===================================================================
--- code/gazebo/trunk/server/Common.hh 2010-04-19 22:39:25 UTC (rev 8614)
+++ code/gazebo/trunk/server/Common.hh 2010-04-23 23:57:16 UTC (rev 8615)
@@ -51,9 +51,12 @@
/// \return Name of the entity
public: std::string GetName() const;
- /// \brief Get the parameters
- public: std::vector<Param*> *GetParams();
+ /// \brief Get the count of the parameters
+ public: unsigned int GetParamCount() const;
+ /// \brief Get a param by index
+ public: Param *GetParam(unsigned int index) const;
+
/// \brief Get a parameter by name
public: Param *GetParam(const std::string &key) const;
Modified: code/gazebo/trunk/server/Factory.cc
===================================================================
--- code/gazebo/trunk/server/Factory.cc 2010-04-19 22:39:25 UTC (rev 8614)
+++ code/gazebo/trunk/server/Factory.cc 2010-04-23 23:57:16 UTC (rev 8615)
@@ -111,7 +111,8 @@
// Attempt to delete a model by name, if the string is present
if (strcmp((const char*)this->factoryIface->data->deleteModel,"")!=0)
{
- World::Instance()->DeleteEntity((const
char*)this->factoryIface->data->deleteModel);
+ const std::string m = (const char*)this->factoryIface->data->deleteModel;
+ World::Instance()->DeleteEntity(m);
strcpy((char*)this->factoryIface->data->deleteModel,"");
}
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2010-04-19 22:39:25 UTC (rev 8614)
+++ code/gazebo/trunk/server/Model.cc 2010-04-23 23:57:16 UTC (rev 8615)
@@ -165,6 +165,7 @@
scopedName = this->GetScopedName();
dup = World::Instance()->GetEntityByName(scopedName);
+
// Look for existing models by the same name
if(dup != NULL && dup != this)
{
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2010-04-19 22:39:25 UTC (rev 8614)
+++ code/gazebo/trunk/server/World.cc 2010-04-23 23:57:16 UTC (rev 8615)
@@ -518,13 +518,25 @@
// maybe try try_lock here instead
boost::recursive_mutex::scoped_lock
lock(*Simulator::Instance()->GetMDMutex());
+
// Remove and delete all models that are marked for deletion
- std::vector< Model* >::iterator miter;
+ std::vector< std::string>::iterator miter;
for (miter=this->toDeleteModels.begin();
miter!=this->toDeleteModels.end(); miter++)
{
- (*miter)->Fini();
- delete (*miter);
+ Entity *entity = this->GetEntityByName(*miter);
+ std::cout << "Process Entity to Delete[" << (*miter) << "]\n";
+ if (entity)
+ {
+ Model *model = dynamic_cast<Model*>(entity);
+ if (model)
+ {
+ model->Fini();
+ this->models.erase( std::find(this->models.begin(),
+ this->models.end(), model) );
+ }
+ delete (entity);
+ }
}
this->toDeleteModels.clear();
@@ -534,19 +546,19 @@
////////////////////////////////////////////////////////////////////////////////
/// Delete an entity by name
-void World::DeleteEntity(const char *name)
+void World::DeleteEntity(const std::string &name)
{
std::vector< Model* >::iterator miter;
- // Update all the models
- for (miter=this->models.begin(); miter!=this->models.end(); miter++)
- {
- if ((*miter)->GetCompleteScopedName() == name)
- {
- this->toDeleteModels.push_back(*miter);
- this->models.erase( std::remove(this->models.begin(),
this->models.end(), *miter));
- }
- }
+ Entity *entity = this->GetEntityByName(name);
+
+ if (!entity)
+ return;
+
+ Model *model = dynamic_cast<Model*>(entity);
+
+ if (model)
+ this->toDeleteModels.push_back(model->GetName());
}
@@ -950,6 +962,70 @@
break;
}
+ case SimulationRequestData::GET_ENTITY_PARAM_KEY:
+ {
+ Entity *entity = this->GetEntityByName((char*)req->name);
+ if (entity)
+ {
+ Param *param = entity->GetParam(req->uintValue);
+ response->type= req->type;
+ memset(response->strValue, 0, 512);
+
+ if (param)
+ strncpy(response->strValue, param->GetKey().c_str(), 512);
+
+ response->strValue[511] = '\0';
+
+ response++;
+ this->simIface->data->responseCount += 1;
+ }
+ else
+ gzerr(0) << "Invalid entity name[" << req->name << "] in
simulation interface GET_ENTITY_PARAM.\n";
+
+ break;
+ }
+
+ case SimulationRequestData::GET_ENTITY_PARAM_VALUE:
+ {
+ Entity *entity = this->GetEntityByName((char*)req->name);
+ if (entity)
+ {
+ Param *param = entity->GetParam(req->uintValue);
+ response->type= req->type;
+ memset(response->strValue, 0, 512);
+
+ if (param)
+ strncpy(response->strValue, param->GetAsString().c_str(), 512);
+
+ response->strValue[511] = '\0';
+ response++;
+ this->simIface->data->responseCount += 1;
+ }
+ else
+ gzerr(0) << "Invalid entity name[" << req->name << "] in
simulation interface GET_ENTITY_PARAM.\n";
+
+ break;
+ }
+
+
+
+ case SimulationRequestData::GET_ENTITY_PARAM_COUNT:
+ {
+ Entity *entity = this->GetEntityByName((char*)req->name);
+ if (entity)
+ {
+ unsigned int count = entity->GetParamCount();
+ response->type= req->type;
+ response->uintValue = count;
+ response++;
+ this->simIface->data->responseCount += 1;
+ }
+ else
+ gzerr(0) << "Invalid entity name[" << req->name << "] in
simulation interface GET_ENTITY_PARAM_COUNT.\n";
+
+ break;
+ }
+
case SimulationRequestData::GET_MODEL_NAME:
{
unsigned int index = req->uintValue;
Modified: code/gazebo/trunk/server/World.hh
===================================================================
--- code/gazebo/trunk/server/World.hh 2010-04-19 22:39:25 UTC (rev 8614)
+++ code/gazebo/trunk/server/World.hh 2010-04-23 23:57:16 UTC (rev 8615)
@@ -155,7 +155,7 @@
/// \brief Delete an entity by name
/// \param name The name of the entity to delete
- public: void DeleteEntity(const char *name);
+ public: void DeleteEntity(const std::string &name);
/// \brief Get a pointer to a entity based on a name
public: Entity *GetEntityByName(const std::string &name) const;
@@ -344,7 +344,7 @@
private: std::vector<Model*> models;
/// List of models to delete from the world
- private: std::vector< Model* > toDeleteModels;
+ private: std::vector< std::string > toDeleteModels;
private: std::vector< std::string > toLoadEntities;
Modified: code/gazebo/trunk/server/gui/MainMenu.cc
===================================================================
--- code/gazebo/trunk/server/gui/MainMenu.cc 2010-04-19 22:39:25 UTC (rev
8614)
+++ code/gazebo/trunk/server/gui/MainMenu.cc 2010-04-23 23:57:16 UTC (rev
8615)
@@ -51,10 +51,10 @@
{
{ "File", 0, 0, 0, FL_SUBMENU, FL_NORMAL_LABEL, 0, 14, 0 },
// { "Open", 0, &gazebo::MainMenu::OpenCB, 0, 0, FL_NORMAL_LABEL,0, 14,0
},
- { "Save World", 0, &gazebo::MainMenu::SaveWorldCB, 0, 0,
FL_NORMAL_LABEL,0, 14,0 },
+ { "Save World", FL_CTRL+'s', &gazebo::MainMenu::SaveWorldCB, 0, 0,
FL_NORMAL_LABEL,0, 14,0 },
{ "Save Frames", 0, &gazebo::MainMenu::SaveFramesCB, 0, FL_MENU_TOGGLE,
FL_NORMAL_LABEL,0, 14,0 },
- { "Reset", 0, &gazebo::MainMenu::ResetCB, 0, 0, FL_NORMAL_LABEL,0, 14,0 },
- { "Quit", 0, &gazebo::MainMenu::QuitCB, 0, 0, FL_NORMAL_LABEL,0, 14,0 },
+ { "Reset", FL_CTRL+'r', &gazebo::MainMenu::ResetCB, 0, 0,
FL_NORMAL_LABEL,0, 14,0 },
+ { "Quit", FL_CTRL+'q', &gazebo::MainMenu::QuitCB, 0, 0, FL_NORMAL_LABEL,0,
14,0 },
{ 0 },
{ "View", 0, 0, 0, FL_SUBMENU, FL_NORMAL_LABEL, 0, 14, 0},
Modified: code/gazebo/trunk/server/gui/Sidebar.cc
===================================================================
--- code/gazebo/trunk/server/gui/Sidebar.cc 2010-04-19 22:39:25 UTC (rev
8614)
+++ code/gazebo/trunk/server/gui/Sidebar.cc 2010-04-23 23:57:16 UTC (rev
8615)
@@ -351,23 +351,20 @@
// Add entity to browser
void Sidebar::AddEntityToParamBrowser(Common *entity, std::string prefix)
{
- std::vector<Param*> *parameters;
- std::vector<Param*>::iterator iter;
std::string value;
std::string colorStr = "";
- parameters = entity->GetParams();
-
// Process all the parameters in the entity
- for (iter = parameters->begin(); iter != parameters->end(); iter++)
+ for (unsigned int i=0; i < entity->GetParamCount(); i++)
{
+ Param *param = entity->GetParam(i);
/*if ( i%2 == 0)
colorStr = "@B50";
*/
- value = colorStr + "@b...@s" + prefix + (*iter)->GetKey() + ":~" +
- colorStr + "@s" + (*iter)->GetAsString();
+ value = colorStr + "@b...@s" + prefix + param->GetKey() + ":~" +
+ colorStr + "@s" + param->GetAsString();
this->AddToParamBrowser( value );
}
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2010-04-19 22:39:25 UTC (rev
8614)
+++ code/gazebo/trunk/server/physics/Body.cc 2010-04-23 23:57:16 UTC (rev
8615)
@@ -55,6 +55,7 @@
Body::Body(Entity *parent)
: Entity(parent)
{
+ this->type = Entity::BODY;
this->comEntity = new Entity(this);
this->comEntity->SetName("COM Entity");
Modified: code/gazebo/trunk/server/physics/ode/ODEGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEGeom.cc 2010-04-19 22:39:25 UTC
(rev 8614)
+++ code/gazebo/trunk/server/physics/ode/ODEGeom.cc 2010-04-23 23:57:16 UTC
(rev 8615)
@@ -138,7 +138,7 @@
// Save the body based on our XMLConfig node
void ODEGeom::Save(std::string &prefix, std::ostream &stream)
{
- if (this->GetType() == Shape::RAY)
+ if (this->GetShapeType() == Shape::RAY)
return;
Geom::Save(prefix, stream);
Modified: code/gazebo/trunk/tools/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/tools/CMakeLists.txt 2010-04-19 22:39:25 UTC (rev
8614)
+++ code/gazebo/trunk/tools/CMakeLists.txt 2010-04-23 23:57:16 UTC (rev
8615)
@@ -8,4 +8,4 @@
add_executable(gazebomodel gazebomodel.cc)
-target_link_libraries(gazebomodel gazebo boost_program_options)
+target_link_libraries(gazebomodel gazebo boost_program_options yaml)
Modified: code/gazebo/trunk/tools/gazebomodel.cc
===================================================================
--- code/gazebo/trunk/tools/gazebomodel.cc 2010-04-19 22:39:25 UTC (rev
8614)
+++ code/gazebo/trunk/tools/gazebomodel.cc 2010-04-23 23:57:16 UTC (rev
8615)
@@ -1,12 +1,21 @@
#include <iostream>
-//#include <gazebo/gazebo.h>
+#include <stdio.h>
+#include <sys/select.h>
+#include <boost/algorithm/string.hpp>
#include "libgazebo/gz.h"
+#include <yaml.h>
+
gazebo::Client *client = NULL;
gazebo::SimulationIface *simIface = NULL;
gazebo::FactoryIface *factoryIface = NULL;
+std::map<std::string, std::string> yamlValues;
+std::vector<std::string> params;
+yaml_parser_t parser;
+yaml_event_t event;
+
////////////////////////////////////////////////////////////////////////////////
// Print out info for one model. Recurses through all child models
void print_model(std::string name, std::string prefix)
@@ -14,19 +23,7 @@
std::string type;
gazebo::Pose pose;
- /*if (!simIface->GetPose3d(name, pose))
- std::cerr << "Unable to get model[" << name << "] pose\n";
- if (!simIface->GetModelType(name, type))
- std::cerr << "Unable to get model[" << name << "] type\n";
- */
-
std::cout << prefix << name << "\n";
- /*std::cout << prefix << " Type: " << type << "\n";
- std::cout << prefix << " XYZ: " << pose.pos.x << " " << pose.pos.y
- << " " << pose.pos.z << "\n";
- std::cout << prefix << " RPY: " << pose.roll << " " << pose.pitch
- << " " << pose.yaw << "\n";
- */
unsigned int children;
if (!simIface->GetNumChildren(name, children))
@@ -51,7 +48,7 @@
////////////////////////////////////////////////////////////////////////////////
// Print out a list of all the models
-void list(int argc, char **argv)
+void list()
{
unsigned int numModels = 0;
@@ -71,26 +68,93 @@
////////////////////////////////////////////////////////////////////////////////
// Show info for a model
-void show(int argc, char **argv)
+void show()
{
- if (argc < 3)
+ if (params.size() < 2)
std::cerr << "Missing model name\n";
else
- print_model(argv[2], "");
+ {
+ for (unsigned int i=1; i < params.size(); i++)
+ {
+ std::string name = params[i];
+ std::string type;
+ gazebo::Pose pose;
+ unsigned int paramCount;
+
+ if (!simIface->GetPose3d(name, pose))
+ std::cerr << "Unable to get model[" << name << "] pose\n";
+ if (!simIface->GetModelType(name, type))
+ std::cerr << "Unable to get model[" << name << "] type\n";
+ if (!simIface->GetEntityParamCount(name, paramCount))
+ std::cerr << "Unable to get model[" << name << "] param count\n";
+
+ for (unsigned int i=0; i < paramCount; i++)
+ {
+ std::string paramKey;
+ std::string paramValue;
+
+ if (!simIface->GetEntityParamKey(name, i, paramKey))
+ std::cerr << "Unable to get model[" << name << "] param key\n";
+ if (!simIface->GetEntityParamValue(name, i, paramValue))
+ std::cerr << "Unable to get model[" << name << "] param value\n";
+
+ std::cout << paramKey << ": " << paramValue << "\n";
+ }
+ std::cout << "\n";
+ }
+ }
}
////////////////////////////////////////////////////////////////////////////////
// Remove a model from the world
-void kill(int argc, char **argv)
+void kill()
{
- if (argc < 3)
+ if (params.size() < 2)
std::cerr << "Missing model name\n";
else
{
- factoryIface->DeleteModel( argv[2] );
+ // Kill all the passed in models
+ for (unsigned int i=1; i < params.size(); i++)
+ {
+ factoryIface->DeleteModel( params[i] );
+
+ while (strcmp((const char*)factoryIface->data->deleteModel,"") != 0)
+ usleep(10000);
+ }
}
}
+////////////////////////////////////////////////////////////////////////////////
+// Spawn a new model into the world
+void spawn()
+{
+ //TODO
+ //PROCESS YAML PARAMETER!!!
+ if (params.size() < 2)
+ std::cerr << "Missing model filename\n";
+ else
+ {
+ FILE *file = fopen(params[1].c_str(),"r");
+ if (file)
+ {
+ std::ostringstream stream;
+ while (!feof(file))
+ {
+ char buffer[256];
+ fgets(buffer, 256, file);
+ if (feof(file))
+ break;
+ stream << buffer;
+ }
+ strcpy((char*)factoryIface->data->newModel, stream.str().c_str());
+ }
+ else
+ std::cerr << "Unable to open file[" << params[1] << "]\n";
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Print out help information
void help()
{
std::cout << "gazebomodel is a command-line tool for printing out
information about models in a gazebo world.\n";
@@ -100,9 +164,59 @@
std::cout << "Commands:\n";
std::cout << "\tgazebomodel list \t List all the models\n";
- std::cout << "\tgazebomodel show \t Show info about a model\n";
+ std::cout << "\tgazebomodel show \t Show info about a model(s)\n";
+ std::cout << "\tgazebomodel kill \t Remove a model(s) from the world\n";
+ std::cout << "\tgazebomodel spawn \t Insert a model into the world\n";
}
+////////////////////////////////////////////////////////////////////////////////
+// Parse yaml parameters
+void parseYAML()
+{
+ std::map<std::string, std::string>::iterator iter;
+
+ // Create the yaml parser
+ yaml_parser_initialize(&parser);
+
+ std::string input = params[params.size()-1];
+ yaml_parser_set_input_string(&parser,
+ (const unsigned char *)input.c_str(), input.size());
+
+ bool done = false;
+ std::string name;
+ while (!done)
+ {
+ if (!yaml_parser_parse(&parser, &event))
+ {
+ std::cerr << "YAML error: Bad syntax for '" << input << "'\n";
+ break;
+ }
+
+ if (event.type == YAML_SCALAR_EVENT)
+ {
+ if (name.size() == 0)
+ name = (char*)(event.data.scalar.value);
+ else
+ {
+ yamlValues[name] = (char*)event.data.scalar.value;
+ name = "";
+ }
+ }
+
+ done = (event.type == YAML_STREAM_END_EVENT);
+ yaml_event_delete(&event);
+ }
+
+ yaml_parser_delete(&parser);
+
+ /*for (iter = yamlValues.begin(); iter != yamlValues.end(); iter++)
+ {
+ std::cout << "Key[" << iter->first << "] Value[" << iter->second << "]\n";
+ }*/
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Main
int main(int argc, char **argv)
{
@@ -112,6 +226,32 @@
return 1;
}
+ // Get parameters from command line
+ for (int i=1; i < argc; i++)
+ {
+ std::string p = argv[i];
+ boost::trim(p);
+ params.push_back( p );
+ }
+
+ // Get parameters from stdin
+ if (!isatty(fileno(stdin)))
+ {
+ char str[1024];
+ while (!feof(stdin))
+ {
+ fgets(str, 1024, stdin);
+ if (feof(stdin))
+ break;
+ std::string p = str;
+ boost::trim(p);
+ params.push_back(p);
+ }
+ }
+
+ parseYAML();
+
+ /*
client = new gazebo::Client();
simIface = new gazebo::SimulationIface();
factoryIface = new gazebo::FactoryIface();
@@ -149,13 +289,15 @@
return -1;
}
- std::string cmd = argv[1];
- if (cmd == "list")
- list(argc, argv);
- else if (cmd == "show")
- show(argc, argv);
- else if (cmd == "kill")
- kill(argc, argv);
+ if (params[0] == "list")
+ list();
+ else if (params[0] == "show")
+ show();
+ else if (params[0] == "kill")
+ kill();
+ else if (params[0] == "spawn")
+ spawn();
else
- std::cerr << "Unknown command[" << cmd << "]\n";
+ std::cerr << "Unknown command[" << params[0] << "]\n";
+ */
}
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
