Revision: 7917
http://playerstage.svn.sourceforge.net/playerstage/?rev=7917&view=rev
Author: asadat
Date: 2009-06-27 00:08:44 +0000 (Sat, 27 Jun 2009)
Log Message:
-----------
added some methods to get the child interfaces of a model/body/sensor.
Modified Paths:
--------------
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/Model.hh
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/World.hh
code/gazebo/trunk/server/controllers/Controller.cc
code/gazebo/trunk/server/controllers/Controller.hh
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/Body.hh
code/gazebo/trunk/server/sensors/Sensor.cc
code/gazebo/trunk/server/sensors/Sensor.hh
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2009-06-27 00:06:41 UTC (rev 7916)
+++ code/gazebo/trunk/server/Model.cc 2009-06-27 00:08:44 UTC (rev 7917)
@@ -49,6 +49,7 @@
#include "IfaceFactory.hh"
#include "Model.hh"
+
#ifdef TIMING
#include "Simulator.hh"
#endif
@@ -1108,3 +1109,47 @@
}
}
+
+
+/////////////////////////////////////////////////////////////////////////////////////
+// Get the list of model interfaces e.g
"pioneer2dx_model1::laser::laser_iface0->laser"
+void Model::GetModelInterfaceNames(std::vector<std::string>& list) const{
+
+
+ //std::vector<Entity*>::iterator eiter;
+ std::map<std::string, Controller* >::const_iterator contIter;
+
+ for (contIter=this->controllers.begin();
+ contIter!=this->controllers.end(); contIter++)
+ {
+ contIter->second->GetInterfaceNames(list);
+
+ }
+
+
+ std::map< std::string, Body* >::const_iterator biter;
+
+ for (biter=this->bodies.begin(); biter != this->bodies.end();
biter++)
+ {
+
+ biter->second->GetInterfaceNames(list);
+
+
+ }
+
+
+
+/*
+ for (eiter=this->GetChildren().begin();
eiter!=this->GetChildren().end(); eiter++)
+ {
+ Model* m;
+ m = dynamic_cast<Model*>(*eiter);
+ if(m)
+ m->GetModelInterfaceNames(list);
+
+ }
+*/
+
+}
+
+
Modified: code/gazebo/trunk/server/Model.hh
===================================================================
--- code/gazebo/trunk/server/Model.hh 2009-06-27 00:06:41 UTC (rev 7916)
+++ code/gazebo/trunk/server/Model.hh 2009-06-27 00:08:44 UTC (rev 7917)
@@ -39,6 +39,7 @@
#include "Entity.hh"
#include "gazebo.h"
+
namespace boost
{
class recursive_mutex;
@@ -190,6 +191,9 @@
/// \brief Set the laser retro reflectiveness of the model
public: void SetLaserRetro( const float &retro );
+ /// \brief Get the list of interfaces e.g
"pioneer2dx_model1::laser::laser_iface0->laser"
+ public: void GetModelInterfaceNames(std::vector<std::string>& list) const;
+
/// \brief Load a body helper function
/// \param node XML Configuration node
private: void LoadBody(XMLConfigNode *node);
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2009-06-27 00:06:41 UTC (rev 7916)
+++ code/gazebo/trunk/server/World.cc 2009-06-27 00:08:44 UTC (rev 7917)
@@ -920,6 +920,177 @@
break;
}
+ case SimulationRequestData::GET_INTERFACE_TYPE:
+ {
+ //printf("Model Type Request\n");
+ std::vector<std::string> list;
+
+ response->type = req->type;
+ strcpy( response->modelName, req->modelName);
+ std::vector<Model*>::iterator miter;
+
+ for (miter=models.begin(); miter!=models.end(); miter++)
+ GetInterfaceNames((*miter), list);
+
+ std::string mname = req->modelName;
+ unsigned int i=mname.find(".");
+ while(i!= std::string::npos){
+
+ mname.erase(i,1);
+ mname.insert(i,"::");
+ i= mname.find(".");
+ }
+
+
+ std::vector<std::string> candids;
+
+ for(unsigned int j=0;j<list.size();j++){
+
+ int ind = list[j].find(mname);
+ if(ind==0 && ind!=std::string::npos && list[j].size() >
mname.size()){
+
candids.push_back(list[j].substr(ind+mname.size(),list[j].size()-ind-mname.size()));
+ }
+ }
+
+ /*for(unsigned int ii=0;ii<candids.size();ii++)
+ printf("candidatetypes: %s\n",candids[ii].c_str());*/
+
+ for(i=0; i<candids.size(); i++){
+ if(candids[i][0]=='>'){
+
+
strcpy(response->strValue,candids[i].substr(2,candids[i].size()-2).c_str());
+ response->strValue[511]='\0';
+ i=candids.size()+5;
+
+ }
+ }
+
+ if(strcmp(response->strValue,"irarray")==0){
+ strcpy(response->strValue,"ranger");
+ response->strValue[511]='\0';
+ }
+
+ if(i<candids.size()+4) // the model is not an interface
+ {
+
+ strcpy(response->strValue,"unkown");
+ response->strValue[511]='\0';
+
+ }
+
+ //printf("-> modeltype: %s \n", response->modelType);
+
+ response++;
+ this->simIface->data->responseCount += 1;
+
+ break;
+ }
+
+ case SimulationRequestData::GET_MODEL_INTERFACES:
+ {
+
+ //printf("Requested the children\n");
+ //printf("-> %s", req->modelName);
+
+
+ response->nChildInterfaces=0;
+ //std::vector<Entity*>::iterator iter;
+ std::vector<std::string> list;
+
+ response->type = req->type;
+ strcpy( response->modelName, req->modelName);
+ std::vector<Model*>::iterator miter;
+
+ for (miter=models.begin(); miter!=models.end(); miter++)
+ GetInterfaceNames((*miter), list);
+ /*
+ for(unsigned int ii=0;ii<list.size();ii++)
+ printf("interface: %s\n",list[ii].c_str());
+ */
+ // removing the ">>type" from the end of each interface names
+ for(unsigned int jj=0;jj<list.size();jj++){
+ unsigned int index = list[jj].find(">>");
+ if(index !=std::string::npos)
+ list[jj].replace(index,list[jj].size(),"");
+ //printf("-->> %s \n",list[jj].c_str());
+ }
+
+
+ if(strcmp((char*)req->modelName,"")==0){
+
+ std::vector<std::string> chlist;
+ for(unsigned int i=0;i<list.size();i++){
+
+ std::string str = list[i].substr(0,list[i].find("::"));
+ std::vector<std::string>::iterator itr;
+ itr = std::find(chlist.begin(),chlist.end(), str);
+
+ if(itr!=chlist.end() || str=="")
+ continue;
+
+ chlist.push_back(str);
+
strcpy(response->childInterfaces[response->nChildInterfaces++],str.c_str());
+
response->childInterfaces[response->nChildInterfaces-1][511]='\0';
+ }
+
+
+ }else{
+
+
+ std::vector<std::string> newlist;
+ std::string mname = (char*)req->modelName;
+
+ unsigned int i=mname.find(".");
+ while(i>-1){
+
+ mname.erase(i,1);
+ mname.insert(i,"::");
+ i= mname.find(".");
+ }
+
+ for(unsigned int j=0;j<list.size();j++){
+
+ unsigned int ind = list[j].find(mname);
+ if(ind==0 && ind!=std::string::npos &&
list[j].size() > mname.size()){
+
newlist.push_back(list[j].substr(ind+mname.size()+2,list[j].size()-ind-mname.size()-2));
+ }
+ }
+
+ /*for(unsigned int ii=0;ii<newlist.size();ii++)
+ printf("child interface:
%s\n",newlist[ii].c_str());
+*/
+ std::vector<std::string> chlist;
+ for( i=0;i<newlist.size();i++){
+
+ unsigned int indx = newlist[i].find("::");
+ indx =
(indx==std::string::npos)?newlist[i].size():indx;
+ std::string str = newlist[i].substr(0,indx);
+ std::vector<std::string>::iterator itr;
+ itr = std::find(chlist.begin(),chlist.end(),
str);
+
+ if(itr!=chlist.end() || str=="")
+ continue;
+
+ chlist.push_back(str);
+ // Adding the parent name to the child name e.g
"parent.child"
+ str=mname+"::"+str;
+
+
strcpy(response->childInterfaces[response->nChildInterfaces++],str.c_str());
+
response->childInterfaces[response->nChildInterfaces-1][511]='\0';
+ }
+
+
+
+
+
+ }
+
+ response++;
+ this->simIface->data->responseCount += 1;
+
+
+ break;
+ }
case SimulationRequestData::GO:
{
this->simPauseTime = Simulator::Instance()->GetSimTime()
@@ -946,7 +1117,7 @@
}
else
{
- gzerr(0) << "Invalid model name[" << req->modelName << "] in
simulation interface Set Pose 2d Request.\n";
+ gzerr(0) << "Invalid model name[" << req->modelName << "] in
simulation interface Get Children Request.\n";
}
break;
}
@@ -983,3 +1154,25 @@
this->toDeleteModels.clear();
}
+void World::GetInterfaceNames(Entity* en, std::vector<std::string>& list){
+
+
+ Model* m = dynamic_cast<Model*>(en);
+ if(m){
+ m->GetModelInterfaceNames(list);
+ }
+
+ std::vector<Entity*>::iterator citer;
+ for (citer=en->GetChildren().begin(); citer!=en->GetChildren().end();
citer++)
+ {
+
+
+ this->GetInterfaceNames((*citer),list);
+
+
+ }
+
+}
+
+
+
Modified: code/gazebo/trunk/server/World.hh
===================================================================
--- code/gazebo/trunk/server/World.hh 2009-06-27 00:06:41 UTC (rev 7916)
+++ code/gazebo/trunk/server/World.hh 2009-06-27 00:08:44 UTC (rev 7917)
@@ -34,6 +34,7 @@
#include "SingletonT.hh"
#include "Vector3.hh"
#include "Pose3d.hh"
+#include "Entity.hh"
namespace gazebo
{
@@ -202,6 +203,9 @@
/// \brief Update the simulation iface
private: void UpdateSimulationIface();
+ /// \brif Get the names of interfaces defined in the tree of a model
+ private: void GetInterfaceNames(Entity* m, std::vector<std::string>& list);
+
/// Pointer the physics engine
private: PhysicsEngine *physicsEngine;
Modified: code/gazebo/trunk/server/controllers/Controller.cc
===================================================================
--- code/gazebo/trunk/server/controllers/Controller.cc 2009-06-27 00:06:41 UTC
(rev 7916)
+++ code/gazebo/trunk/server/controllers/Controller.cc 2009-06-27 00:08:44 UTC
(rev 7917)
@@ -288,3 +288,18 @@
}
return iface;
}
+
+void Controller::GetInterfaceNames(std::vector<std::string>& list) const{
+
+ std::vector<Iface*>::const_iterator iter;
+
+ for (iter = this->ifaces.begin(); iter != this->ifaces.end(); iter++)
+ {
+ std::string str;
+ str=(*iter)->GetId()+">>"+(*iter)->GetType();
+ list.push_back(str);
+
+ }
+
+
+}
Modified: code/gazebo/trunk/server/controllers/Controller.hh
===================================================================
--- code/gazebo/trunk/server/controllers/Controller.hh 2009-06-27 00:06:41 UTC
(rev 7916)
+++ code/gazebo/trunk/server/controllers/Controller.hh 2009-06-27 00:08:44 UTC
(rev 7917)
@@ -99,6 +99,8 @@
/// \return The name of the controller
public: std::string GetName() const;
+ public: void GetInterfaceNames(std::vector<std::string>& list) const;
+
/// \brief Return Iface by type
/// \param type The type of the iface to retrieve
/// \param number If several ifaces of the same type present, which one
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-06-27 00:06:41 UTC (rev
7916)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-06-27 00:08:44 UTC (rev
7917)
@@ -1289,6 +1289,19 @@
return this->sensors;
}
+////////////////////////////////////////////////////////////////////////////////////////
+/// \brief Get the list of interfaces e.g
"pioneer2dx_model1::laser::laser_iface0->laser"
+void Body::GetInterfaceNames(std::vector<std::string>& list) const{
+
+ std::vector< Sensor* >::const_iterator iter;
+
+ for (iter = this->sensors.begin(); iter != this->sensors.end(); iter++)
+ {
+ (*iter)->GetInterfaceNames(list);
+ }
+
+}
+
////////////////////////////////////////////////////////////////////////////////
/// Get the size of the body
void Body::GetBoundingBox(Vector3 &min, Vector3 &max ) const
Modified: code/gazebo/trunk/server/physics/Body.hh
===================================================================
--- code/gazebo/trunk/server/physics/Body.hh 2009-06-27 00:06:41 UTC (rev
7916)
+++ code/gazebo/trunk/server/physics/Body.hh 2009-06-27 00:08:44 UTC (rev
7917)
@@ -192,6 +192,9 @@
/// \brief Get the mass of the body
public: float GetMass() const { return mass.mass; }
+ /// \brief Get the list of interfaces e.g
"pioneer2dx_model1::laser::laser_iface0->laser"
+ public: void GetInterfaceNames(std::vector<std::string>& list) const;
+
/// \brief Get a sensor by name
public: Sensor *GetSensor( const std::string &name ) const;
Modified: code/gazebo/trunk/server/sensors/Sensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/Sensor.cc 2009-06-27 00:06:41 UTC (rev
7916)
+++ code/gazebo/trunk/server/sensors/Sensor.cc 2009-06-27 00:08:44 UTC (rev
7917)
@@ -229,3 +229,9 @@
{
return this->body->GetPose();
}
+///////////////////////////////////////////////////////////////////////////////
+/// Get the name of the interfaces define in the sensor controller
+void Sensor::GetInterfaceNames(std::vector<std::string>& list) const
+{
+ controller->GetInterfaceNames(list);
+}
Modified: code/gazebo/trunk/server/sensors/Sensor.hh
===================================================================
--- code/gazebo/trunk/server/sensors/Sensor.hh 2009-06-27 00:06:41 UTC (rev
7916)
+++ code/gazebo/trunk/server/sensors/Sensor.hh 2009-06-27 00:08:44 UTC (rev
7917)
@@ -69,8 +69,14 @@
/// \brief Finalize the sensor
public: void Fini();
+ /// \brief Get the type of the sensor
+ public: std::string GetSensorType(){return typeName;}
+
/// \brief Get the current pose
public: virtual Pose3d GetPose() const;
+
+ /// \brief Get the name of the interfaces define in the sensor controller
+ public: void GetInterfaceNames(std::vector<std::string>& list) const;
/// \brief Set whether the sensor is active or not
public: void SetActive(bool value);
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
