Update of /cvsroot/playerstage/code/gazebo/server/models
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv11622/server/models
Added Files:
Tag: ogre
Model.cc Model.hh ModelFactory.cc ModelFactory.hh SConscript
Log Message:
Added new gazebo files
--- NEW FILE: SConscript ---
#Import variable
Import('env staticObjs sharedObjs')
dirs = Split('GroundPlane LightSource ObserverCam')
for subdir in dirs :
SConscript('%s/SConscript' % subdir)
sources = Split('Model.cc ModelFactory.cc')
staticObjs.append(env.StaticObject(sources))
sharedObjs.append(env.SharedObject(sources))
--- NEW FILE: Model.hh ---
#ifndef MODEL_HH
#define MODEL_HH
#include <python2.4/Python.h>
#include <map>
#include <string>
#include <vector>
#include "Pose3d.hh"
#include "Joint.hh"
#include "Entity.hh"
#include "gazebo.h"
// Forward declarations
namespace Ogre
{
class SceneNode;
}
class XMLConfigNode;
class Body;
class Model : public Entity
{
// Constructor
public: Model();
// Destructor
public: virtual ~Model();
// Load the model
public: int Load(XMLConfigNode *node);
// Initialize the model
public: int Init();
// Update the model
public: int Update();
// Finalize the model
public: int Fini();
// Load the child model
protected: virtual int LoadChild(XMLConfigNode * /*node*/) {return 0;}
// Initialize the child model
protected: virtual int InitChild() {return 0;}
// Update the child model
protected: virtual int UpdateChild() {return 0;}
// Finilaize thie child model
protected: virtual int FiniChild() {return 0;}
// Set the type of the model
public: void SetType(const std::string &type);
// Get the type of the model
public: const std::string &GetType() const;
// Set the XMLConfig node this model was loaded from
public: void SetXMLConfigNode( XMLConfigNode *node );
// Get the XML Conig node this model was loaded from
public: XMLConfigNode *GetXMLConfigNode() const;
// Set the name
public: void SetName(const std::string &name);
// Get the name
public: const std::string &GetName() const;
// Set the initial pose
public: void SetInitPose(const Pose3d &pose);
// Get the initial pose
public: const Pose3d &GetInitPose() const;
// Set the current pose
public: void SetPose(const Pose3d &pose);
// Get the current pose
public: const Pose3d &GetPose() const;
//! Create and return a new body
/*!
* \return Pointer to a new body.
*/
public: Body *CreateBody();
//! Create and return a new joint
/*!
* \param type Type of the joint.
* \return Pointer to a new joint.
*/
public: Joint *CreateJoint(Joint::Type type);
//! Load a body helper function
/*!
* \param node XML Configuration node
* \return Non-zero on error
*/
private: int LoadBody(XMLConfigNode *node);
//! Load a joint helper function
/*!
* \param node XML Configuration node
* \return Non-zero on error
*/
private: int LoadJoint(XMLConfigNode *node);
//! Load a interface helper function
/*!
* \param node XML Configuration node
* \return Non-zero on error
*/
private: int LoadIface(XMLConfigNode *node);
// Type of the model (such as Pioneer2DX, or SimpleSolid)
private: std::string type;
// Name for this model
private: std::string name;
// The node this model was loaded from
private: XMLConfigNode *node;
// Initial pose of the model
private: Pose3d initPose;
// Current pose
private: Pose3d pose;
//! Map of the bodies. std::string == body_name
protected: std::map<std::string, Body*> bodies;
protected: std::vector<Joint*> joints;
protected: std::vector<Iface*> ifaces;
private: PyObject *pName;
private: PyObject *pModule;
private: PyObject *pFuncUpdate;
};
#endif
--- NEW FILE: ModelFactory.hh ---
/*
* Gazebo - Outdoor Multi-Robot Simulator
* Copyright (C) 2003
* Nate Koenig & Andrew Howard
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/*
* Desc: Factory for creating models
* Author: Andrew Howard
* Date: 18 May 2003
* CVS info: $Id: ModelFactory.hh,v 1.1.2.1 2006/12/16 22:41:15 natepak Exp $
*/
#ifndef MODELFACTORY_HH
#define MODELFACTORY_HH
#include <string>
#include <map>
// Forward declarations
class Model;
// Prototype for model factory functions
typedef Model* (*ModelFactoryFn) ();
/// @brief The model factory; the class is just for namespacing purposes.
class ModelFactory
{
/// @brief Register all known models.
public: static void RegisterAll();
/// @brief Register a model class (called by model registration function).
public: static void RegisterModel(std::string type, std::string classname,
ModelFactoryFn factoryfn);
/// @brief Create a new instance of a model. Used by the world when
/// reading the world file.
public: static Model *NewModel(const std::string &classname);
// A list of registered model classes
private: static std::map<std::string, ModelFactoryFn> models;
};
/// @brief Static model registration macro
///
/// Use this macro to register models with the server.
/// @param name Model type name, as it appears in the world file.
/// @param classname C++ class name for the model.
#define GZ_REGISTER_STATIC(name, classname) \
Model *New##classname() \
{ \
return new classname(); \
} \
void Register##classname() \
{\
ModelFactory::RegisterModel("static", name, New##classname);\
}
/// @brief Plugin model registration macro
///
/// Use this macro to register plugin models with the server.
/// @param name Model type name, as it appears in the world file.
/// @param classname C++ class name for the model.
#define GZ_REGISTER_PLUGIN(name, classname) \
Model *New##classname() \
{ \
return new classname(); \
} \
extern "C" \
{ \
int gazebo_plugin_init(void) \
{ \
ModelFactory::RegisterModel("plugin", name, New##classname); \
return 0;\
}\
}
#endif
--- NEW FILE: ModelFactory.cc ---
/*
* Gazebo - Outdoor Multi-Robot Simulator
* Copyright (C) 2003
* Nate Koenig & Andrew Howard
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/*
* Desc: Factory for creating models
* Author: Andrew Howard
* Date: 18 May 2003
* CVS info: $Id: ModelFactory.cc,v 1.1.2.1 2006/12/16 22:41:15 natepak Exp $
*/
#include "Model.hh"
#include "ModelFactory.hh"
//void RegisterSimpleSolid();
void RegisterGroundPlane();
void RegisterLightSource();
void RegisterObserverCam();
//void RegisterPioneer2DX();
std::map<std::string, ModelFactoryFn> ModelFactory::models;
// Register all known models.
void ModelFactory::RegisterAll()
{
// Register static models (macro generated by autoconf)
//RegisterSimpleSolid();
RegisterGroundPlane();
RegisterLightSource();
RegisterObserverCam();
//RegisterPioneer2DX();
}
// Register a model class. Use by dynamically loaded modules
void ModelFactory::RegisterModel(std::string type, std::string classname,
ModelFactoryFn factoryfn)
{
//std::cout << "registered " << classname << " (" << type << ")\n";
models[classname] = factoryfn;
return;
}
// Create a new instance of a model. Used by the world when reading
// the world file.
Model *ModelFactory::NewModel(const std::string &classname)
{
if (models[classname])
{
return (models[classname]) ();
}
return NULL;
}
--- NEW FILE: Model.cc ---
#include <boost/python.hpp>
#include <Ogre.h>
#include "OgreAdaptor.hh"
#include "XMLConfig.hh"
#include "World.hh"
#include "Body.hh"
#include "HingeJoint.hh"
#include "Hinge2Joint.hh"
#include "BallJoint.hh"
#include "UniversalJoint.hh"
#include "PhysicsEngine.hh"
#include "Model.hh"
////////////////////////////////////////////////////////////////////////////////
// Constructor
Model::Model()
: Entity()
{
this->name = "";
this->type = "";
this->pName = NULL;
this->pModule = NULL;
this->pFuncUpdate = NULL;
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
Model::~Model()
{
this->bodies.clear();
}
////////////////////////////////////////////////////////////////////////////////
// Load the model
int Model::Load(XMLConfigNode *node)
{
if (node->GetName() == "xml")
{
XMLConfigNode *bodyNode = NULL;
XMLConfigNode *jointNode = NULL;
XMLConfigNode *ifaceNode = NULL;
Body *body;
this->SetName(node->GetString("name","",1));
this->SetStatic(node->GetBool("static",false,0));
// Load the bodies
bodyNode = node->GetChildByNSPrefix("body");
while (bodyNode)
{
if (this->LoadBody(bodyNode) != 0)
std::cerr << "Error Loading body[" << bodyNode->GetName() << "]\n";
bodyNode = bodyNode->GetNextByNSPrefix("body");
}
// Load the joints
jointNode = node->GetChildByNSPrefix("joint");
while (jointNode)
{
if (this->LoadJoint(jointNode) != 0)
std::cerr << "Error Loading Joint[" << jointNode->GetName() << "]\n";
jointNode = jointNode->GetNextByNSPrefix("joint");
}
ifaceNode = node->GetChildByNSPrefix("interface");
while (ifaceNode)
{
if (this->LoadIface(ifaceNode) != 0)
std::cerr << "Error Loading Interface[" << ifaceNode->GetName() <<
"]\n";
ifaceNode = ifaceNode->GetNextByNSPrefix("interface");
}
}
// Get the name of the python module
this->pName = PyString_FromString(node->GetString("python","",0).c_str());
//this->pName = PyString_FromString("pioneer2dx");
// Import the python module
if (this->pName)
{
printf("Load pModule\n");
this->pModule = PyImport_Import(this->pName);
Py_DECREF(this->pName);
}
// Get the Update function from the module
if (this->pModule != NULL)
{
printf("Load pFuncUpdate\n");
this->pFuncUpdate = PyObject_GetAttrString(this->pModule, "Update");
if (this->pFuncUpdate && !PyCallable_Check(this->pFuncUpdate))
this->pFuncUpdate = NULL;
}
return this->LoadChild(node);
}
////////////////////////////////////////////////////////////////////////////////
// Initialize the model
int Model::Init()
{
return this->InitChild();
}
////////////////////////////////////////////////////////////////////////////////
// Update the model
int Model::Update()
{
std::map<std::string, Body*>::iterator iter;
for (iter=this->bodies.begin(); iter!=this->bodies.end(); iter++)
{
iter->second->Update();
}
// Call the model's python update function, if one exists
if (this->pFuncUpdate)
{
printf("Python Update\n");
boost::python::call<void>(this->pFuncUpdate, this);
}
return this->UpdateChild();
}
////////////////////////////////////////////////////////////////////////////////
// Finalize the model
int Model::Fini()
{
return this->FiniChild();
}
////////////////////////////////////////////////////////////////////////////////
// Set the name of the model
void Model::SetType(const std::string &type)
{
this->type = type;
}
////////////////////////////////////////////////////////////////////////////////
// Get the name of the model
const std::string &Model::GetType() const
{
return this->type;
}
////////////////////////////////////////////////////////////////////////////////
// Set the XMLConfig node this model was loaded from
void Model::SetXMLConfigNode( XMLConfigNode *node )
{
this->node = node;
}
////////////////////////////////////////////////////////////////////////////////
// Get the XML Conig node this model was loaded from
XMLConfigNode *Model::GetXMLConfigNode() const
{
return this->node;
}
////////////////////////////////////////////////////////////////////////////////
// Set the Name.
void Model::SetName(const std::string &name)
{
this->name = name;
}
////////////////////////////////////////////////////////////////////////////////
// Get the Name
const std::string &Model::GetName() const
{
return this->name;
}
////////////////////////////////////////////////////////////////////////////////
// Set the initial pose
void Model::SetInitPose(const Pose3d &pose)
{
this->pose = pose;
}
////////////////////////////////////////////////////////////////////////////////
// Get the initial pose
const Pose3d &Model::GetInitPose() const
{
return this->pose;
}
////////////////////////////////////////////////////////////////////////////////
// Set the current pose
void Model::SetPose(const Pose3d &pose)
{
Body *body;
std::map<std::string, Body*>::iterator iter;
Pose3d origPose, newPose, bodyPose;
this->pose = pose;
origPose = this->GetPose();
newPose = pose;
for (iter=this->bodies.begin(); iter!=this->bodies.end(); iter++)
{
body = iter->second;
bodyPose = body->GetPose();
// Compute the pose relative to the model
bodyPose = bodyPose - origPose;
// Compute the new pose
bodyPose += newPose;
body->SetPose(bodyPose);
//body->SetLinearVel(Vector3(0,0,0));
//body->SetAngularVel(Vector3(0,0,0));
}
}
////////////////////////////////////////////////////////////////////////////////
// Get the current pose
const Pose3d &Model::GetPose() const
{
return this->pose;
}
////////////////////////////////////////////////////////////////////////////////
// Create and return a new body
Body *Model::CreateBody()
{
// Create a new body
Body *newBody = World::Instance()->GetPhysicsEngine()->CreateBody(this);
return newBody;
}
////////////////////////////////////////////////////////////////////////////////
// Create and return a new joint
Joint *Model::CreateJoint(Joint::Type type)
{
return World::Instance()->GetPhysicsEngine()->CreateJoint(type);
}
////////////////////////////////////////////////////////////////////////////////
// Load a new body helper function
int Model::LoadBody(XMLConfigNode *node)
{
if (!node)
return -1;
// Create a new body
Body *body = this->CreateBody();
// Load the body using the config node. This also loads all of the
// bodies geometries
body->Load(node);
// Store this body
if (this->bodies[body->GetName()])
std::cerr << "Body with name[" << body->GetName() << "] already exists!!\n";
// Store the pointer to this body
this->bodies[body->GetName()] = body;
return 0;
}
////////////////////////////////////////////////////////////////////////////////
// Load a new joint helper function
int Model::LoadJoint(XMLConfigNode *node)
{
if (!node)
return -1;
Joint *joint = NULL;
Body *body1 = this->bodies[node->GetString("body1","",1)];
Body *body2 = this->bodies[node->GetString("body2","",1)];
Body *anchorBody = this->bodies[node->GetString("anchor","",1)];
Vector3 anchorVec = node->GetVector3("anchor",Vector3(0,0,0));
if (!body1)
{
std::cerr << "Couldn't Find Body[" << node->GetString("body1","",1);
return -1;
}
if (!body2)
{
std::cerr << "Couldn't Find Body[" << node->GetString("body2","",1);
return -1;
}
// Create a Hinge Joint
if (node->GetName() == "hinge")
joint = this->CreateJoint(Joint::HINGE);
else if (node->GetName() == "ball")
joint = this->CreateJoint(Joint::BALL);
else if (node->GetName() == "slider")
joint = this->CreateJoint(Joint::SLIDER);
else if (node->GetName() == "hinge2")
joint = this->CreateJoint(Joint::HINGE2);
else if (node->GetName() == "universal")
joint = this->CreateJoint(Joint::UNIVERSAL);
else
{
std::cerr << "Uknown joint[" << node->GetName() << "]\n";
return -1;
}
// Attach two bodies
joint->Attach(body1,body2);
// Set the anchor vector
if (anchorBody)
joint->SetAnchor(anchorBody->GetPosition());
else
joint->SetAnchor(anchorVec);
// Set the axis of the hing joint
if (node->GetName() == "hinge")
{
((HingeJoint*)joint)->SetAxis(node->GetVector3("axis",Vector3(0,0,1)));
}
else if (node->GetName() == "hinge2")
{
((Hinge2Joint*)joint)->SetAxis1(node->GetVector3("axis1",Vector3(0,0,1)));
((Hinge2Joint*)joint)->SetAxis2(node->GetVector3("axis2",Vector3(0,0,1)));
}
else if (node->GetName() == "universal")
{
((UniversalJoint*)joint)->SetAxis1(node->GetVector3("axis1",Vector3(0,0,1)));
((UniversalJoint*)joint)->SetAxis2(node->GetVector3("axis2",Vector3(0,0,1)));
}
// Set joint parameters
joint->SetParam(dParamSuspensionERP, node->GetDouble("erp",0.0,0));
joint->SetParam(dParamSuspensionCFM, node->GetDouble("cfm",0.0,0));
this->joints.push_back(joint);
return 0;
}
////////////////////////////////////////////////////////////////////////////////
// Load a new interface helper function
int Model::LoadIface(XMLConfigNode *node)
{
if (!node)
return -1;
Iface *iface = NULL;
if (node->GetName() == "position2d")
{
/*iface = new PositionIface();
if (iface->Create(World::Instance()->GetGzServer(),
this->GetName().c_str(), "Pioneer2DX", this->GetId(),
this->GetParentId()) != 0)
return -1;
*/
}
else if (node->GetName() == "power")
{
}
else if (node->GetName() == "sonar")
{
}
if (iface)
this->ifaces.push_back(iface);
return 0;
}
-------------------------------------------------------------------------
Take Surveys. Earn Cash. Influence the Future of IT
Join SourceForge.net's Techsay panel and you'll get the chance to share your
opinions on IT & business topics through brief surveys - and earn cash
http://www.techsay.com/default.php?page=join.php&p=sourceforge&CID=DEVDEV
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
