Revision: 8658
http://playerstage.svn.sourceforge.net/playerstage/?rev=8658&view=rev
Author: natepak
Date: 2010-05-04 20:32:16 +0000 (Tue, 04 May 2010)
Log Message:
-----------
Added more functionality to the gazebo tools. Added an improved manipulation
interface
Modified Paths:
--------------
code/gazebo/trunk/libgazebo/FactoryIface.cc
code/gazebo/trunk/libgazebo/SimIface.cc
code/gazebo/trunk/libgazebo/gz.h
code/gazebo/trunk/tools/CMakeLists.txt
code/gazebo/trunk/tools/gazebomodel.cc
code/gazebo/trunk/worlds/simpleshapes.world
Added Paths:
-----------
code/gazebo/trunk/tools/Toolbase.cc
code/gazebo/trunk/tools/Toolbase.hh
code/gazebo/trunk/tools/gazebobody.cc
Modified: code/gazebo/trunk/libgazebo/FactoryIface.cc
===================================================================
--- code/gazebo/trunk/libgazebo/FactoryIface.cc 2010-05-04 20:31:37 UTC (rev
8657)
+++ code/gazebo/trunk/libgazebo/FactoryIface.cc 2010-05-04 20:32:16 UTC (rev
8658)
@@ -4,11 +4,11 @@
using namespace gazebo;
-/// \brief Delete a model by name
-bool FactoryIface::DeleteModel(const std::string &model_name)
+/// Delete a model by name
+bool FactoryIface::DeleteEntity(const std::string &model_name)
{
this->Lock(1);
- strcpy((char*)this->data->deleteModel, model_name.c_str());
+ strcpy((char*)this->data->deleteEntity, model_name.c_str());
this->Unlock();
return true;
}
Modified: code/gazebo/trunk/libgazebo/SimIface.cc
===================================================================
--- code/gazebo/trunk/libgazebo/SimIface.cc 2010-05-04 20:31:37 UTC (rev
8657)
+++ code/gazebo/trunk/libgazebo/SimIface.cc 2010-05-04 20:32:16 UTC (rev
8658)
@@ -94,8 +94,6 @@
/// Close the interface
void SimulationIface::Close()
{
- printf("Close\n");
-
//stop BlockThread
if (this->goAckThread)
{
@@ -144,7 +142,6 @@
////////////////////////////////////////////////////////////////////////////////
void SimulationIface::BlockThread()
{
- printf("Block thread start\n");
try
{
while (true)
@@ -161,11 +158,8 @@
}
catch (boost::thread_interrupted const &)
{
- fprintf(stderr, "blockthread: thread got interrupted....\n");
return;
}
-
- printf("Block thread end\n");
}
////////////////////////////////////////////////////////////////////////////////
@@ -193,6 +187,17 @@
}
////////////////////////////////////////////////////////////////////////////////
+// Step the simulation
+void SimulationIface::Step()
+{
+ this->Lock(1);
+ this->data->responseCount = 0;
+ SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::STEP;
+ this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
/// Reset the simulation
void SimulationIface::Reset()
{
@@ -340,7 +345,8 @@
this->Unlock();
}
-//////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
/// Get then children of a model
void SimulationIface::GetChildInterfaces(const std::string &name)
{
@@ -372,6 +378,7 @@
this->Unlock();
}
+
///////////////////////////////////////////////////////////////////////////////
/// Get the complete state of a model
bool SimulationIface::GetState(const std::string &name, Pose &modelPose,
@@ -403,7 +410,94 @@
return true;
}
+///////////////////////////////////////////////////////////////////////////////
+/// Set the linear velocity
+void SimulationIface::SetLinearVel(const std::string &modelName, Vec3 vel)
+{
+ this->Lock(1);
+ this->data->responseCount = 0;
+ SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
+ request->type = gazebo::SimulationRequestData::SET_LINEAR_VEL;
+ memset(request->name, 0, 512);
+ strncpy(request->name, modelName.c_str(), 512);
+ request->name[511] = '\0';
+
+ if (isnan(vel.x)) vel.x = 0.0;
+ if (isnan(vel.y)) vel.y = 0.0;
+ if (isnan(vel.z)) vel.z = 0.0;
+
+ request->modelLinearVel = vel;
+
+ this->Unlock();
+}
+
+///////////////////////////////////////////////////////////////////////////////
+/// Set the linear acceleration
+void SimulationIface::SetLinearAccel(const std::string &modelName, Vec3 accel)
+{
+ this->Lock(1);
+ this->data->responseCount = 0;
+ SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
+
+ request->type = gazebo::SimulationRequestData::SET_LINEAR_ACCEL;
+ memset(request->name, 0, 512);
+ strncpy(request->name, modelName.c_str(), 512);
+ request->name[511] = '\0';
+
+ if (isnan(accel.x)) accel.x = 0.0;
+ if (isnan(accel.y)) accel.y = 0.0;
+ if (isnan(accel.z)) accel.z = 0.0;
+
+ request->modelLinearAccel = accel;
+
+ this->Unlock();
+}
+
+///////////////////////////////////////////////////////////////////////////////
+/// Set the angular velocity
+void SimulationIface::SetAngularVel(const std::string &modelName, Vec3 vel)
+{
+ this->Lock(1);
+ this->data->responseCount = 0;
+ SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
+
+ request->type = gazebo::SimulationRequestData::SET_ANGULAR_VEL;
+ memset(request->name, 0, 512);
+ strncpy(request->name, modelName.c_str(), 512);
+ request->name[511] = '\0';
+
+ if (isnan(vel.x)) vel.x = 0.0;
+ if (isnan(vel.y)) vel.y = 0.0;
+ if (isnan(vel.z)) vel.z = 0.0;
+
+ request->modelAngularVel = vel;
+
+ this->Unlock();
+}
+
+///////////////////////////////////////////////////////////////////////////////
+/// Set the angular acceleration
+void SimulationIface::SetAngularAccel(const std::string &modelName, Vec3 accel)
+{
+ this->Lock(1);
+ this->data->responseCount = 0;
+ SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
+
+ request->type = gazebo::SimulationRequestData::SET_ANGULAR_ACCEL;
+ memset(request->name, 0, 512);
+ strncpy(request->name, modelName.c_str(), 512);
+ request->name[511] = '\0';
+
+ if (isnan(accel.x)) accel.x = 0.0;
+ if (isnan(accel.y)) accel.y = 0.0;
+ if (isnan(accel.z)) accel.z = 0.0;
+
+ request->modelAngularAccel = accel;
+
+ this->Unlock();
+}
+
////////////////////////////////////////////////////////////////////////////////
// Wait for a post on the go ack semaphore
void SimulationIface::GoAckWait()
@@ -726,3 +820,34 @@
return true;
}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set a param value for an entity
+bool SimulationIface::SetEntityParamValue(const std::string &entityName,
+ const std::string ¶mName,
+ const std::string ¶mValue )
+{
+ this->Lock(1);
+
+ this->data->responseCount = 0;
+ SimulationRequestData *request;
+
+ request = &(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::SET_ENTITY_PARAM_VALUE;
+
+ memset(request->name, 0, 512);
+ strncpy(request->name, entityName.c_str(), 512);
+ request->name[511] = '\0';
+
+ memset(request->strValue, 0, 512);
+ strncpy(request->strValue, paramName.c_str(), 512);
+ request->strValue[511] = '\0';
+
+ memset(request->strValue1, 0, 512);
+ strncpy(request->strValue1, paramValue.c_str(), 512);
+ request->strValue1[511] = '\0';
+
+ this->Unlock();
+
+ return true;
+}
Modified: code/gazebo/trunk/libgazebo/gz.h
===================================================================
--- code/gazebo/trunk/libgazebo/gz.h 2010-05-04 20:31:37 UTC (rev 8657)
+++ code/gazebo/trunk/libgazebo/gz.h 2010-05-04 20:32:16 UTC (rev 8658)
@@ -420,6 +420,7 @@
public: enum Type { PAUSE,
UNPAUSE,
RESET,
+ STEP,
SAVE,
GET_POSE3D,
GET_POSE2D,
@@ -427,6 +428,10 @@
SET_POSE2D,
SET_STATE,
GET_STATE,
+ SET_LINEAR_VEL,
+ SET_LINEAR_ACCEL,
+ SET_ANGULAR_VEL,
+ SET_ANGULAR_ACCEL,
GO,
GET_ENTITY_TYPE,
GET_ENTITY_PARAM_COUNT,
@@ -441,11 +446,13 @@
GET_MODEL_EXTENT,
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,
};
public: Type type;
public: char name[512];
public: char strValue[512];
+ public: char strValue1[512];
public: Vec3 vec3Value;
public: unsigned int uintValue;
@@ -544,6 +551,9 @@
/// \brief Unpause the simulation
public: void Unpause();
+ /// \brief Step the simulation
+ public: void Step();
+
/// \brief Reset the simulation
public: void Reset();
@@ -568,14 +578,26 @@
/// \brief Set the complete state of a model
public: void SetState(const std::string &modelName, Pose &modelPose,
- Vec3 &linearVel, Vec3 &angularVel,
- Vec3 &linearAccel, Vec3 &angularAccel );
+ Vec3 &linearVel, Vec3 &angularVel,
+ Vec3 &linearAccel, Vec3 &angularAccel );
/// \brief Get the complete state of a model
public: bool GetState(const std::string &modelName, Pose &modelPose,
- Vec3 &linearVel, Vec3 &angularVel,
- Vec3 &linearAccel, Vec3 &angularAccel );
+ Vec3 &linearVel, Vec3 &angularVel,
+ Vec3 &linearAccel, Vec3 &angularAccel );
+ /// \brief Set the linear velocity
+ public: void SetLinearVel(const std::string &modelName, Vec3 vel);
+
+ /// \brief Set the linear acceleration
+ public: void SetLinearAccel(const std::string &modelName,Vec3 acce);
+
+ /// \brief Set the angular velocity
+ public: void SetAngularVel(const std::string &modelName,Vec3 vel);
+
+ /// \brief Set the angular acceleration
+ public: void SetAngularAccel(const std::string &modelName,Vec3 accel);
+
/// \brief Get the child interfaces of a model
public: void GetChildInterfaces(const std::string &modelName);
@@ -620,6 +642,10 @@
public: bool GetEntityParamValue(const std::string &entityName,
unsigned int paramIndex, std::string ¶mValue );
+ /// \brief Set a param value for an entity
+ public: bool SetEntityParamValue(const std::string &entityName,
+ const std::string ¶mName,
+ const std::string ¶mValue );
public: void GoAckWait();
public: void GoAckPost();
@@ -1210,8 +1236,8 @@
/// String describing the model to be initiated
public: uint8_t newModel[409600];
- /// Delete a model by name
- public: uint8_t deleteModel[512];
+ /// Delete an entity by name
+ public: uint8_t deleteEntity[512];
};
/// \brief Factory interface
@@ -1237,8 +1263,8 @@
this->data = (FactoryData*)this->mMap;
}
- /// \brief Delete a model by name
- public: bool DeleteModel(const std::string &model_name);
+ /// \brief Delete an entity by name
+ public: bool DeleteEntity(const std::string &model_name);
/// Pointer to the factory data
public: FactoryData *data;
Modified: code/gazebo/trunk/tools/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/tools/CMakeLists.txt 2010-05-04 20:31:37 UTC (rev
8657)
+++ code/gazebo/trunk/tools/CMakeLists.txt 2010-05-04 20:32:16 UTC (rev
8658)
@@ -2,10 +2,14 @@
link_directories(../libgazebo)
include_directories(../libgazebo)
-set (sources gazebomodel.cc)
+set (sources gazebomodel.cc gazebobody.cc Toolbase.cc)
set_source_files_properties(${sources} PROPERTIES COMPILE_FLAGS "-ggdb -g2
-Wall")
-add_executable(gazebomodel gazebomodel.cc)
+add_executable(gazebomodel gazebomodel.cc Toolbase.cc)
+add_executable(gazebobody gazebobody.cc Toolbase.cc)
-target_link_libraries(gazebomodel gazebo boost_program_options yaml)
+target_link_libraries(gazebomodel gazebo boost_program_options yaml
boost_regex)
+target_link_libraries(gazebobody gazebo boost_program_options yaml boost_regex)
+
+install (TARGETS gazebomodel gazebobody DESTINATION
${CMAKE_INSTALL_PREFIX}/bin)
Added: code/gazebo/trunk/tools/Toolbase.cc
===================================================================
--- code/gazebo/trunk/tools/Toolbase.cc (rev 0)
+++ code/gazebo/trunk/tools/Toolbase.cc 2010-05-04 20:32:16 UTC (rev 8658)
@@ -0,0 +1,217 @@
+#include <iostream>
+#include <boost/algorithm/string.hpp>
+#include <boost/lexical_cast.hpp>
+
+#include "Toolbase.hh"
+
+////////////////////////////////////////////////////////////////////////////////
+/// Constructor
+Toolbase::Toolbase()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Init the tool
+bool Toolbase::Init(int argc, char **argv)
+{
+ if (argc == 1 || std::string(argv[1]) == "help")
+ {
+ this->Help();
+ return false;
+ }
+
+ // Get parameters from command line
+ for (int i=1; i < argc; i++)
+ {
+ std::string p = argv[i];
+ boost::trim(p);
+ this->params.push_back( p );
+ }
+
+ // Get parameters from stdin
+ if (!isatty(fileno(stdin)))
+ {
+ char str[1024];
+ while (!feof(stdin))
+ {
+ if (fgets(str, 1024, stdin)==NULL)
+ break;
+
+ if (feof(stdin))
+ break;
+ std::string p = str;
+ boost::trim(p);
+ this->params.push_back(p);
+ }
+ }
+
+ this->ParseYAML();
+
+ this->client = new gazebo::Client();
+ this->simIface = new gazebo::SimulationIface();
+ this->factoryIface = new gazebo::FactoryIface();
+
+ try
+ {
+ this->client->ConnectWait(0, GZ_CLIENT_ID_USER_FIRST);
+ }
+ catch(std::string e)
+ {
+ std::cerr << "Gazebo Error: Unable to connect: " << e << "\n";
+ return false;
+ }
+
+ /// Open the sim iface
+ try
+ {
+ this->simIface->Open(client, "default");
+ }
+ catch (std::string e)
+ {
+ std::cerr << "Gazebo error: Unable to connect to sim iface:" << e << "\n";
+ return false;
+ }
+
+ // Open the factory iface
+ try
+ {
+ this->factoryIface->Open(client, "default");
+ }
+ catch( std::string e)
+ {
+ std::cerr << "Gazebo error: Unable to connect to the factory Iface:"
+ << e << "\n";
+ return false;
+ }
+
+ return true;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Destructor
+Toolbase::~Toolbase()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Run
+bool Toolbase::Run()
+{
+ bool handeled = true;
+
+ if (this->params[0] == "pause")
+ this->Pause();
+ else if (this->params[0] == "go")
+ this->Unpause();
+ else if (this->params[0] == "reset")
+ this->Reset();
+ else if (this->params[0] == "step")
+ this->Step();
+ else
+ handeled = false;
+
+ return handeled;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Pause the simulation
+void Toolbase::Pause()
+{
+ this->simIface->Pause();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Run the simulation
+void Toolbase::Unpause()
+{
+ this->simIface->Unpause();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Step the simulation
+void Toolbase::Step()
+{
+ int count = 1;
+
+ if (params.size() >= 2)
+ {
+ try
+ {
+ count = boost::lexical_cast<int>(params[1]);
+ }
+ catch (boost::bad_lexical_cast &e)
+ {
+ std::cerr << "step can take only an integer parameter\n";
+ }
+ }
+
+ std::cout << "COUNT[" << count << "]\n";
+ for (int i=0; i < count; i++)
+ {
+ this->simIface->Step();
+ usleep(1000);
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Pause the simulation
+void Toolbase::Reset()
+{
+ this->simIface->Reset();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Parse yaml parameters
+void Toolbase::ParseYAML()
+{
+
+ // 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);
+
+ std::map<std::string, std::string>::iterator iter;
+ for (iter = yamlValues.begin(); iter != yamlValues.end(); iter++)
+ {
+ std::cout << "Key[" << iter->first << "] Value[" << iter->second << "]\n";
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Print all the commands to stdout
+void Toolbase::PrintCommands(std::string prefix)
+{
+ std::cout << "\t" << prefix << " pause \t Pause the simulation\n";
+ std::cout << "\t" << prefix << " go \t\t Unpause the simulation\n";
+ std::cout << "\t" << prefix << " step \t Increment the simulation time\n";
+ std::cout << "\t" << prefix << " reset \t Reset the simulation\n";
+}
Added: code/gazebo/trunk/tools/Toolbase.hh
===================================================================
--- code/gazebo/trunk/tools/Toolbase.hh (rev 0)
+++ code/gazebo/trunk/tools/Toolbase.hh 2010-05-04 20:32:16 UTC (rev 8658)
@@ -0,0 +1,57 @@
+#ifndef TOOLBASE_HH
+#define TOOLBASE_HH
+
+#include <map>
+#include <vector>
+#include <yaml.h>
+
+#include "libgazebo/gz.h"
+
+class Toolbase
+{
+ /// \brief Constructor
+ public: Toolbase();
+
+ /// \brief Destructor
+ public: virtual ~Toolbase();
+
+ /// \brief Init the tool
+ public: bool Init(int argc, char **argv);
+
+ /// \brief Run the tool
+ public: virtual bool Run();
+
+ /// \brief Pause the simulation
+ public: void Pause();
+
+ /// \brief Run the simulation
+ public: void Unpause();
+
+ /// \brief Step the simulation
+ public: void Step();
+
+ /// \brief Pause the simulation
+ public: void Reset();
+
+ public: virtual void Help() = 0;
+
+ /// \brief Parse yaml parameters
+ private: void ParseYAML();
+
+ /// \brief Print all the commands to stdout
+ protected: void PrintCommands(std::string prefix);
+
+ protected: gazebo::Client *client;
+ protected: gazebo::SimulationIface *simIface;
+ protected: gazebo::FactoryIface *factoryIface;
+
+ protected: std::map<std::string, std::string> yamlValues;
+ protected: std::vector<std::string> params;
+
+ protected: yaml_parser_t parser;
+ protected: yaml_event_t event;
+
+
+};
+
+#endif
Added: code/gazebo/trunk/tools/gazebobody.cc
===================================================================
--- code/gazebo/trunk/tools/gazebobody.cc (rev 0)
+++ code/gazebo/trunk/tools/gazebobody.cc 2010-05-04 20:32:16 UTC (rev
8658)
@@ -0,0 +1,126 @@
+#include <iostream>
+#include <stdio.h>
+//#include <sys/select.h>
+#include <boost/regex.hpp>
+#include <boost/algorithm/string.hpp>
+#include <boost/lexical_cast.hpp>
+
+#include "Toolbase.hh"
+
+class BodyTool : public Toolbase
+{
+
+
//////////////////////////////////////////////////////////////////////////////
+ // Print out info for one body. Recurses through all child models
+ public: void PrintBody(std::string name, std::string prefix)
+ {
+ std::string type;
+ gazebo::Pose pose;
+
+ this->simIface->GetEntityType(name, type);
+ if (type == "body")
+ std::cout << name << "\n";
+
+ unsigned int children;
+ if (!this->simIface->GetNumChildren(name, children))
+ std::cerr << "Unable to get the number of children for model["
+ << name << "]\n";
+
+ for (unsigned int i=0; i < children; i++)
+ {
+ std::string childName;
+ if (!this->simIface->GetChildName(name, i, childName))
+ std::cerr << "Unable to get model[" << name << "] child name.\n";
+ else
+ {
+ this->PrintBody(childName, prefix+" ");
+ }
+ }
+ }
+
+
//////////////////////////////////////////////////////////////////////////////
+ // Print out a list of all the bodies
+ public: void List()
+ {
+ unsigned int numModels = 0;
+
+ std::string prefix = "";
+ if (!this->simIface->GetNumModels(numModels))
+ std::cerr << "Unable to get the model count\n";
+
+ for (unsigned int i=0; i < numModels; i++)
+ {
+ std::string name;
+ if (!this->simIface->GetModelName(i, name))
+ std::cerr << "Unable to get model[" << i << "]\n";
+
+ this->PrintBody(name, "");
+ }
+ }
+
+
//////////////////////////////////////////////////////////////////////////////
+ // Remove a body from the world
+ public: void Kill()
+ {
+ if (this->params.size() < 2)
+ std::cerr << "Missing model name\n";
+ else
+ {
+ // Kill all the passed in models
+ for (unsigned int i=1; i < this->params.size(); i++)
+ {
+ this->factoryIface->DeleteEntity( params[i] );
+
+ while (strcmp((const char*)this->factoryIface->data->deleteEntity,"")
!= 0)
+ usleep(10000);
+ }
+ }
+ }
+ public: bool Run()
+ {
+ if (!Toolbase::Run())
+ {
+ if (this->params[0] == "list")
+ this->List();
+ else if (params[0] == "kill")
+ this->Kill();
+ else
+ std::cerr << "Unknown command[" << this->params[0] << "]\n";
+ }
+
+ return true;
+ }
+
+
+
//////////////////////////////////////////////////////////////////////////////
+ // Print out help information
+ public: void Help()
+ {
+ std::cout << "gazebobody is a command-line tool for manipulating bodies in
a gazebo world.\n";
+ std::cout << "\n";
+ std::cout << "Usage: gazebobody <command> <option_1> ... <option_n>\n";
+ std::cout << "\n";
+
+ std::cout << "Commands:\n";
+
+ this->PrintCommands("gazebobody");
+
+ std::cout << "\tgazebobody list \t List all the models\n";
+ std::cout << "\tgazebobody info \t Show info about a model(s)\n";
+ std::cout << "\tgazebobody kill \t Remove a model(s) from the world\n";
+ std::cout << "\tgazebobody spawn \t Insert a model into the world\n";
+ std::cout << "\tgazebobody set \t Get a parameter's value\n";
+ }
+};
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Main
+int main(int argc, char **argv)
+{
+ BodyTool tool;
+ tool.Init(argc, argv);
+ tool.Run();
+
+ return 1;
+}
Modified: code/gazebo/trunk/tools/gazebomodel.cc
===================================================================
--- code/gazebo/trunk/tools/gazebomodel.cc 2010-05-04 20:31:37 UTC (rev
8657)
+++ code/gazebo/trunk/tools/gazebomodel.cc 2010-05-04 20:32:16 UTC (rev
8658)
@@ -1,310 +1,267 @@
#include <iostream>
#include <stdio.h>
-#include <sys/select.h>
+#include <boost/regex.hpp>
#include <boost/algorithm/string.hpp>
-#include "libgazebo/gz.h"
+#include <boost/lexical_cast.hpp>
-#include <yaml.h>
+#include "Toolbase.hh"
-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)
+class ModelTool : public Toolbase
{
- std::string type;
- gazebo::Pose pose;
- std::cout << prefix << name << "\n";
+
//////////////////////////////////////////////////////////////////////////////
+ // Print out info for one model. Recurses through all child models
+ public: void PrintModel(std::string name, std::string prefix)
+ {
+ std::string type;
+ gazebo::Pose pose;
- unsigned int children;
- if (!simIface->GetNumChildren(name, children))
- std::cerr << "Unable to get the number of children for model["
- << name << "]\n";
+ std::cout << prefix << name << "\n";
- for (unsigned int i=0; i < children; i++)
- {
- std::string childName;
- if (!simIface->GetChildName(name, i, childName))
- std::cerr << "Unable to get model[" << name << "] child name.\n";
- else
+ unsigned int children;
+ if (!this->simIface->GetNumChildren(name, children))
+ std::cerr << "Unable to get the number of children for model["
+ << name << "]\n";
+
+ for (unsigned int i=0; i < children; i++)
{
- std::string type;
- simIface->GetEntityType(childName, type);
- if (type == "model")
- print_model(childName, prefix+" ");
+ std::string childName;
+ if (!this->simIface->GetChildName(name, i, childName))
+ std::cerr << "Unable to get model[" << name << "] child name.\n";
+ else
+ {
+ std::string type;
+ this->simIface->GetEntityType(childName, type);
+ if (type == "model")
+ this->PrintModel(childName, prefix+" ");
+ }
}
}
-}
+
//////////////////////////////////////////////////////////////////////////////
+ // Print out a list of all the models
+ public: void List()
+ {
+ unsigned int numModels = 0;
-////////////////////////////////////////////////////////////////////////////////
-// Print out a list of all the models
-void list()
-{
- unsigned int numModels = 0;
+ std::string prefix = "";
+ if (!this->simIface->GetNumModels(numModels))
+ std::cerr << "Unable to get the model count\n";
- std::string prefix = "";
- if (!simIface->GetNumModels(numModels))
- std::cerr << "Unable to get the model count\n";
+ for (unsigned int i=0; i < numModels; i++)
+ {
+ std::string name;
+ if (!this->simIface->GetModelName(i, name))
+ std::cerr << "Unable to get model[" << i << "]\n";
- for (unsigned int i=0; i < numModels; i++)
- {
- std::string name;
- if (!simIface->GetModelName(i, name))
- std::cerr << "Unable to get model[" << i << "]\n";
-
- print_model(name, "");
+ this->PrintModel(name, "");
+ }
}
-}
-////////////////////////////////////////////////////////////////////////////////
-// Show info for a model
-void show()
-{
- if (params.size() < 2)
- std::cerr << "Missing model name\n";
- else
+
//////////////////////////////////////////////////////////////////////////////
+ // Show info for a model
+ public: void Info()
{
- for (unsigned int i=1; i < params.size(); i++)
+ if (this->params.size() < 2)
+ std::cerr << "Missing model name\n";
+ else
{
- std::string name = params[i];
- std::string type;
- gazebo::Pose pose;
- unsigned int paramCount;
+ for (unsigned int i=1; i < this->params.size(); i++)
+ {
+ std::string name = params[i];
+ std::string type;
+ gazebo::Pose pose;
+ gazebo::Vec3 linearVel, linearAccel, angularVel, angularAccel;
+ 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";
+ if (!this->simIface->GetModelType(name, type))
+ std::cerr << "Unable to get model[" << name << "] type\n";
+ if (!this->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;
+ this->simIface->GetState(name, pose, linearVel, angularVel,
+ linearAccel, angularAccel);
- 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 << "linear_vel: " << linearVel.x << " "
+ << linearVel.y << " " << linearVel.z << "\n";
+ std::cout << "linear_accel: " << linearAccel.x << " "
+ << linearAccel.y << " " << linearAccel.z << "\n";
- std::cout << paramKey << ": " << paramValue << "\n";
+ std::cout << "angular_vel: " << angularVel.x << " "
+ << angularVel.y << " " << angularVel.z << "\n";
+ std::cout << "angular_accel: " << angularAccel.x << " "
+ << angularAccel.y << " " << angularAccel.z << "\n";
+
+ for (unsigned int i=0; i < paramCount; i++)
+ {
+ std::string paramKey;
+ std::string paramValue;
+
+ if (!this->simIface->GetEntityParamKey(name, i, paramKey))
+ std::cerr << "Unable to get model[" << name << "] param key\n";
+ if (!this->simIface->GetEntityParamValue(name, i, paramValue))
+ std::cerr << "Unable to get model[" << name << "] param value\n";
+
+ std::cout << paramKey << ": " << paramValue << "\n";
+ }
+ std::cout << "\n";
}
- std::cout << "\n";
}
}
-}
-////////////////////////////////////////////////////////////////////////////////
-// Remove a model from the world
-void kill()
-{
- if (params.size() < 2)
- std::cerr << "Missing model name\n";
- else
+
//////////////////////////////////////////////////////////////////////////////
+ // Remove a model from the world
+ public: void Kill()
{
- // Kill all the passed in models
- for (unsigned int i=1; i < params.size(); i++)
+ if (this->params.size() < 2)
+ std::cerr << "Missing model name\n";
+ else
{
- factoryIface->DeleteModel( params[i] );
+ // Kill all the passed in models
+ for (unsigned int i=1; i < this->params.size(); i++)
+ {
+ this->factoryIface->DeleteEntity( params[i] );
- while (strcmp((const char*)factoryIface->data->deleteModel,"") != 0)
- usleep(10000);
+ while (strcmp((const char*)this->factoryIface->data->deleteEntity,"")
!= 0)
+ usleep(10000);
+ }
}
}
-}
-////////////////////////////////////////////////////////////////////////////////
-// Spawn a new model into the world
-void spawn()
-{
-
- if (params.size() < 2)
- std::cerr << "Missing model filename\n";
- else
+
//////////////////////////////////////////////////////////////////////////////
+ // Spawn a new model into the world
+ public: void Spawn()
{
- FILE *file = fopen(params[1].c_str(),"r");
- if (file)
+ std::map<std::string, std::string>::iterator iter;
+
+ if (this->params.size() < 2)
+ std::cerr << "Missing model filename\n";
+ else
{
- std::ostringstream stream;
- while (!feof(file))
+ FILE *file = fopen(params[1].c_str(),"r");
+ if (file)
{
- char buffer[256];
- if (fgets(buffer, 256, file) == NULL)
- std::cerr << "Unable to read file\n";
+ std::ostringstream stream;
+ while (!feof(file))
+ {
+ char buffer[256];
+ if (fgets(buffer, 256, file) == NULL)
+ break;
- if (feof(file))
+ if (feof(file))
break;
- stream << buffer;
- }
- std::string model = stream.str();
- strcpy((char*)factoryIface->data->newModel, model.c_str());
+ stream << buffer;
+ }
+ std::string model = stream.str();
- /*for (iter = yamlValues.begin(); iter != yamlValues.end(); iter++)
- {
- std::string reg = iter->first + "
- boost::regex::regex_replace(model,
- }*/
- }
- else
- std::cerr << "Unable to open file[" << params[1] << "]\n";
- }
-}
+ for (iter = this->yamlValues.begin();
+ iter != this->yamlValues.end(); iter++)
+ {
+ boost::regex e1(std::string("(<model.*?") + iter->first +
"\\h*?=\\h*?)\".*?\"(.*?<body)");
+ boost::regex e2(std::string("(<model.*?<") + iter->first +
")>.*?</(" + iter->first + ">.*?<body)");
-////////////////////////////////////////////////////////////////////////////////
-// 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";
- std::cout << "\n";
- std::cout << "Usage: gazebomodel <command> <option_1> ... <option_n>\n";
- std::cout << "\n";
+ std::string fmt1 = std::string("$1\"") + iter->second + "\"$2";
+ std::string fmt2 = std::string("$1>") + iter->second + "</$2";
- std::cout << "Commands:\n";
- std::cout << "\tgazebomodel list \t List all the models\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";
-}
+ model = boost::regex_replace(model,e1, fmt1,
boost::format_first_only);
+ model = boost::regex_replace(model,e2, fmt2,
boost::format_first_only);
+ }
-////////////////////////////////////////////////////////////////////////////////
-// Parse yaml parameters
-void parseYAML()
-{
- std::map<std::string, std::string>::iterator iter;
+ strcpy((char*)this->factoryIface->data->newModel, model.c_str());
+ }
+ else
+ std::cerr << "Unable to open file[" << this->params[1] << "]\n";
+ }
+ }
- // 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)
+
//////////////////////////////////////////////////////////////////////////////
+ //
+ public: void Set()
{
- if (!yaml_parser_parse(&parser, &event))
+ if (this->params.size() < 2)
+ std::cerr << "Missing model name\n";
+ else
{
- std::cerr << "YAML error: Bad syntax for '" << input << "'\n";
- break;
- }
+ std::string name = this->params[1];
- if (event.type == YAML_SCALAR_EVENT)
- {
- if (name.size() == 0)
- name = (char*)(event.data.scalar.value);
- else
+ std::map<std::string, std::string>::iterator iter;
+ for (iter = this->yamlValues.begin();
+ iter != this->yamlValues.end(); iter++)
{
- yamlValues[name] = (char*)event.data.scalar.value;
- name = "";
+ if (iter->first == "linear_vel" || iter->first == "linear_accel" ||
+ iter->first == "angular_vel" || iter->first == "angular_accel")
+ {
+ std::vector<std::string> strs;
+ boost::split(strs, iter->second, boost::is_any_of("\t "));
+ gazebo::Vec3 vec( boost::lexical_cast<float>(strs[0]),
+ boost::lexical_cast<float>(strs[1]),
+ boost::lexical_cast<float>(strs[2]));
+
+ if (iter->first == "linear_vel")
+ this->simIface->SetLinearVel(name, vec);
+ else if (iter->first == "linear_accel")
+ this->simIface->SetLinearAccel(name, vec);
+ else if (iter->first == "angular_vel")
+ this->simIface->SetAngularVel(name, vec);
+ else
+ this->simIface->SetAngularAccel(name, vec);
+ }
+ else
+ this->simIface->SetEntityParamValue(name, iter->first, iter->second
);
}
}
-
- done = (event.type == YAML_STREAM_END_EVENT);
- yaml_event_delete(&event);
}
- yaml_parser_delete(&parser);
-
- for (iter = yamlValues.begin(); iter != yamlValues.end(); iter++)
+ public: bool Run()
{
- std::cout << "Key[" << iter->first << "] Value[" << iter->second << "]\n";
- }
-}
+ if (!Toolbase::Run())
+ {
+ if (this->params[0] == "list")
+ this->List();
+ else if (params[0] == "info")
+ this->Info();
+ else if (params[0] == "kill")
+ this->Kill();
+ else if (params[0] == "spawn")
+ this->Spawn();
+ else if (params[0] == "set")
+ this->Set();
+ else
+ std::cerr << "Unknown command[" << this->params[0] << "]\n";
+ }
-////////////////////////////////////////////////////////////////////////////////
-// Main
-int main(int argc, char **argv)
-{
-
- if (argc == 1 || std::string(argv[1]) == "help")
- {
- help();
- return 1;
+ return true;
}
- // 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)))
+
//////////////////////////////////////////////////////////////////////////////
+ // Print out help information
+ public: void Help()
{
- char str[1024];
- while (!feof(stdin))
- {
- if (fgets(str, 1024, stdin)==NULL)
- std::cerr << "Unable to read file\n";
- if (feof(stdin))
- break;
- std::string p = str;
- boost::trim(p);
- params.push_back(p);
- }
- }
+ std::cout << "gazebomodel is a command-line tool for printing out
information about models in a gazebo world.\n";
+ std::cout << "\n";
+ std::cout << "Usage: gazebomodel <command> <option_1> ... <option_n>\n";
+ std::cout << "\n";
- parseYAML();
+ std::cout << "Commands:\n";
- client = new gazebo::Client();
- simIface = new gazebo::SimulationIface();
- factoryIface = new gazebo::FactoryIface();
+ this->PrintCommands("gazebomodel");
- try
- {
- client->ConnectWait(0, GZ_CLIENT_ID_USER_FIRST);
+ std::cout << "\tgazebomodel list \t List all the models\n";
+ std::cout << "\tgazebomodel info \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";
+ std::cout << "\tgazebomodel set \t Get a parameter's value\n";
}
- catch(std::string e)
- {
- std::cerr << "Gazebo Error: Unable to connect: " << e << "\n";
- return -1;
- }
+};
- /// Open the sim iface
- try
- {
- simIface->Open(client, "default");
- }
- catch (std::string e)
- {
- std::cerr << "Gazebo error: Unable to connect to sim iface:" << e << "\n";
- return -1;
- }
+////////////////////////////////////////////////////////////////////////////////
+// Main
+int main(int argc, char **argv)
+{
+ ModelTool tool;
+ tool.Init(argc, argv);
+ tool.Run();
- // Open the factory iface
- try
- {
- factoryIface->Open(client, "default");
- }
- catch( std::string e)
- {
- std::cerr << "Gazebo error: Unable to connect to the factory Iface:"
- << e << "\n";
- return -1;
- }
-
- 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[" << params[0] << "]\n";
+ return 1;
}
Modified: code/gazebo/trunk/worlds/simpleshapes.world
===================================================================
--- code/gazebo/trunk/worlds/simpleshapes.world 2010-05-04 20:31:37 UTC (rev
8657)
+++ code/gazebo/trunk/worlds/simpleshapes.world 2010-05-04 20:32:16 UTC (rev
8658)
@@ -47,8 +47,8 @@
</rendering:ogre>
<model:physical name="sphere1_model">
- <xyz>1 0 0.5</xyz>
- <rpy>0.0 0.0 0.0</rpy>
+ <xyz>0 0 0.2</xyz>
+ <rpy>45 0.0 0.0</rpy>
<static>false</static>
<body:sphere name="sphere1_body">
@@ -61,6 +61,7 @@
<visual>
<size>0.4 0.4 0.4</size>
<mesh>unit_sphere</mesh>
+ <shader>pixel</shader>
<material>Gazebo/Rocky</material>
</visual>
</geom:sphere>
@@ -88,7 +89,7 @@
<!-- White Point light -->
<model:renderable name="point_white">
- <xyz>-2 2 5</xyz>
+ <xyz>-2 2 10</xyz>
<static>false</static>
<light>
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
