Revision: 7016 http://playerstage.svn.sourceforge.net/playerstage/?rev=7016&view=rev Author: natepak Date: 2008-09-09 22:04:34 +0000 (Tue, 09 Sep 2008)
Log Message: ----------- Added contact sensor, bumper controller, and bumper interface Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/libgazebo/Iface.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/GazeboDriver.cc code/gazebo/trunk/player/SConscript code/gazebo/trunk/server/controllers/SConscript code/gazebo/trunk/server/physics/ContactParams.hh code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/physics/ode/ODEPhysics.cc code/gazebo/trunk/server/sensors/SConscript Added Paths: ----------- code/gazebo/trunk/player/BumperInterface.cc code/gazebo/trunk/player/BumperInterface.hh code/gazebo/trunk/server/controllers/bumper/ code/gazebo/trunk/server/controllers/bumper/SConscript code/gazebo/trunk/server/controllers/bumper/generic/ code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.cc code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.hh code/gazebo/trunk/server/controllers/bumper/generic/SConscript code/gazebo/trunk/server/sensors/contact/ code/gazebo/trunk/server/sensors/contact/ContactSensor.cc code/gazebo/trunk/server/sensors/contact/ContactSensor.hh code/gazebo/trunk/server/sensors/contact/SConscript code/gazebo/trunk/worlds/bumper.world Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/SConstruct 2008-09-09 22:04:34 UTC (rev 7016) @@ -45,6 +45,7 @@ '#server/sensors', '#server/sensors/camera', '#server/sensors/ray', + '#server/sensors/contact', '#server/physics', '#server/physics/ode', '#server/controllers', Modified: code/gazebo/trunk/libgazebo/Iface.cc =================================================================== --- code/gazebo/trunk/libgazebo/Iface.cc 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/libgazebo/Iface.cc 2008-09-09 22:04:34 UTC (rev 7016) @@ -58,6 +58,7 @@ GZ_REGISTER_IFACE("ptz", PTZIface); GZ_REGISTER_IFACE("stereocamera", StereoCameraIface); GZ_REGISTER_IFACE("opaque", OpaqueIface); +GZ_REGISTER_IFACE("bumper", BumperIface); ////////////////////////////////////////////////////////////////////////////// // Create an interface Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-09-09 22:04:34 UTC (rev 7016) @@ -1411,9 +1411,66 @@ /** \} */ /// \} + /***************************************************************************/ /// \addtogroup libgazebo_iface /// \{ +/** \defgroup bumper_iface Bumper + + \brief Bumper interface + +The bumper interface allows a client to read data from a bumper/contact sensor +\{ +*/ + +#define GAZEBO_MAX_BUMPER_COUNT 128 + +/// \brief Bumper data +class BumperData +{ + public: GazeboData head; + + /// State of the bumpers + public: uint8_t bumpers[GAZEBO_MAX_BUMPER_COUNT]; + + /// Bumper count + public: unsigned int bumper_count; +}; + +/// \brief Bumper interface +class BumperIface : public Iface +{ + /// \brief Constructor + public: BumperIface():Iface("bumper", sizeof(BumperIface)+sizeof(BumperData)) {} + + /// \brief Destructor + public: virtual ~BumperIface() {this->data = NULL;} + + /// \brief Create the server + public: virtual void Create(Server *server, std::string id) + { + Iface::Create(server,id); + this->data = (BumperData*)this->mMap; + } + + /// \brief Open the iface + public: virtual void Open(Client *client, std::string id) + { + Iface::Open(client,id); + this->data = (BumperData*)this->mMap; + } + + /// Pointer to the bumper data + public: BumperData *data; +}; + +/** \} */ +/// \} + + +/***************************************************************************/ +/// \addtogroup libgazebo_iface +/// \{ /** \defgroup opaque_iface opaque \brief Interface for arbitrary data transfer Added: code/gazebo/trunk/player/BumperInterface.cc =================================================================== --- code/gazebo/trunk/player/BumperInterface.cc (rev 0) +++ code/gazebo/trunk/player/BumperInterface.cc 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,140 @@ +/* + * 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: Bumper Interface for Player + * Author: Nate Koenig + * Date: 09 Sept. 2008 + * CVS: $Id:$ + */ + +/** [EMAIL PROTECTED] player [EMAIL PROTECTED] Bumper Interface +- PLAYER_BUMPER_DATA_STATE +*/ + +/* TODO +- PLAYER_BUMPER_REQ_GET_GEOM +*/ + +#include <iostream> +#include <math.h> + +#include "gazebo.h" +#include "GazeboDriver.hh" +#include "BumperInterface.hh" + +using namespace gazebo; + +/////////////////////////////////////////////////////////////////////////////// +// Constructor +BumperInterface::BumperInterface(player_devaddr_t addr, + GazeboDriver *driver, ConfigFile *cf, + int section) + : GazeboInterface(addr, driver, cf, section) +{ + // Get the ID of the interface + this->gz_id = (char*) calloc(1024, sizeof(char)); + strcat(this->gz_id, GazeboClient::prefixId); + strcat(this->gz_id, cf->ReadString(section, "gz_id", "")); + + // Allocate a Position Interface + this->iface = new BumperIface(); + + this->datatime = -1; + + this->data.bumpers_count = 1; + this->data.bumpers = new uint8_t[1]; +} + +/////////////////////////////////////////////////////////////////////////////// +// Destructor +BumperInterface::~BumperInterface() +{ + // Release this interface + delete this->iface; + + delete [] this->data.bumpers; +} + +/////////////////////////////////////////////////////////////////////////////// +// Handle all messages. This is called from GazeboDriver +int BumperInterface::ProcessMessage(QueuePointer &respQueue, + player_msghdr_t *hdr, void *data) +{ + return -1; +} + +/////////////////////////////////////////////////////////////////////////////// +// Update this interface, publish new info. This is +// called from GazeboDriver::Update +void BumperInterface::Update() +{ + this->iface->Lock(1); + + // Only Update when new data is present + if (this->iface->data->head.time > this->datatime) + { + this->datatime = this->iface->data->head.time; + + if (this->data.bumpers_count < this->iface->data->bumper_count) + { + delete [] this->data.bumpers; + this->data.bumpers = new uint8_t[this->iface->data->bumper_count]; + } + + // Copy bumper data + this->data.bumpers_count = this->iface->data->bumper_count; + memcpy( this->data.bumpers, this->iface->data->bumpers, + sizeof(uint8_t) * this->data.bumpers_count ); + + this->driver->Publish( this->device_addr, + PLAYER_MSGTYPE_DATA, + PLAYER_BUMPER_DATA_STATE, + (void*)&data, sizeof(data), &this->datatime ); + } + + this->iface->Unlock(); +} + + +/////////////////////////////////////////////////////////////////////////////// +// Open a SHM interface when a subscription is received. This is called from +// GazeboDriver::Subscribe +void BumperInterface::Subscribe() +{ + try + { + this->iface->Open( GazeboClient::client, this->gz_id); + } + catch (std::string e) + { + std::cerr << "Error subscribing to Gazebo Bumper Interface\n" + << e << "\n"; + exit(0); + } +} + +/////////////////////////////////////////////////////////////////////////////// +// Close a SHM interface. This is called from GazeboDriver::Unsubscribe +void BumperInterface::Unsubscribe() +{ + this->iface->Close(); +} Added: code/gazebo/trunk/player/BumperInterface.hh =================================================================== --- code/gazebo/trunk/player/BumperInterface.hh (rev 0) +++ code/gazebo/trunk/player/BumperInterface.hh 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,83 @@ +/* + * 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: Bumper Interface for Player + * Author: Nate Koenig + * Date: 09 Sept. 2008 + * SVN: $Id:$ + */ + +#ifndef BUMPERINTERFACE_HH +#define BUMPERINTERFACE_HH + +#include "GazeboInterface.hh" + +namespace gazebo +{ + /// \addtogroup player_iface + /// \{ + /// \defgroup gripper_player Bumper Interface + /// \brief Bumper Player interface + /// \{ + + class BumperIface; + + /// \brief Bumper interface + class BumperInterface : public GazeboInterface + { + /// \brief Constructor + public: BumperInterface(player_devaddr_t addr, GazeboDriver *driver, + ConfigFile *cf, int section); + + /// \brief Destructor + public: virtual ~BumperInterface(); + + /// \brief Handle all messages. This is called from GazeboDriver + public: virtual int ProcessMessage(QueuePointer &respQueue, + player_msghdr_t *hdr, void *data); + + /// \brief Update this interface, publish new info. + public: virtual void Update(); + + /// \brief Open a SHM interface when a subscription is received. + /// This is called fromGazeboDriver::Subscribe + public: virtual void Subscribe(); + + /// \brief Close a SHM interface. This is called from + /// GazeboDriver::Unsubscribe + public: virtual void Unsubscribe(); + + private: BumperIface *iface; + + /// \brief Gazebo id. This needs to match and ID in a Gazebo WorldFile + private: char *gz_id; + + /// \brief Timestamp on last data update + private: double datatime; + + private: player_bumper_data_t data; + }; + + /// \} + /// \} + +} + +#endif Modified: code/gazebo/trunk/player/GazeboDriver.cc =================================================================== --- code/gazebo/trunk/player/GazeboDriver.cc 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/player/GazeboDriver.cc 2008-09-09 22:04:34 UTC (rev 7016) @@ -39,6 +39,7 @@ #include "ActarrayInterface.hh" #include "OpaqueInterface.hh" #include "PTZInterface.hh" +#include "BumperInterface.hh" #include "GripperInterface.hh" /* @@ -311,6 +312,12 @@ ifsrc = new GripperInterface( playerAddr, this, cf, section ); break; + case PLAYER_BUMPER_CODE: + if (!player_quiet_startup) printf(" a bumper interface.\n"); + ifsrc = new BumperInterface( playerAddr, this, cf, section ); + break; + + /* case PLAYER_POWER_CODE: if (!player_quiet_startup) printf(" a power interface.\n"); ifsrc = new PowerInterface( playerAddr, this, cf, section ); Modified: code/gazebo/trunk/player/SConscript =================================================================== --- code/gazebo/trunk/player/SConscript 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/player/SConscript 2008-09-09 22:04:34 UTC (rev 7016) @@ -16,7 +16,8 @@ 'PTZInterface.cc', 'OpaqueInterface.cc', 'ActarrayInterface.cc', - 'GripperInterface.cc' + 'GripperInterface.cc', + 'BumperInterface.cc' ] # Position3dInterface.cc PowerInterface.cc SonarInterface.ccGpsInterface.cc ') Modified: code/gazebo/trunk/server/controllers/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/SConscript 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/server/controllers/SConscript 2008-09-09 22:04:34 UTC (rev 7016) @@ -1,7 +1,7 @@ #Import variable Import('env sharedObjs headers') -dirs = Split('position2d laser camera factory gripper actarray ptz opaque') +dirs = Split('position2d laser camera factory gripper actarray ptz opaque bumper') if env['with_audio'] == 'yes': dirs+=Split('audio') Added: code/gazebo/trunk/server/controllers/bumper/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/bumper/SConscript (rev 0) +++ code/gazebo/trunk/server/controllers/bumper/SConscript 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,7 @@ +#Import variable +Import('env sharedObjs') + +dirs = Split('generic') + +for subdir in dirs : + SConscript('%s/SConscript' % subdir) Added: code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.cc =================================================================== --- code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.cc (rev 0) +++ code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.cc 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,99 @@ +/* + * 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: Bumper controller + * Author: Nate Koenig + * Date: 09 Setp. 2008 + */ + +#include "Global.hh" +#include "XMLConfig.hh" +#include "ContactSensor.hh" +#include "World.hh" +#include "gazebo.h" +#include "GazeboError.hh" +#include "ControllerFactory.hh" +#include "Simulator.hh" +#include "Generic_Bumper.hh" + +using namespace gazebo; + +GZ_REGISTER_STATIC_CONTROLLER("bumper", Generic_Bumper); + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +Generic_Bumper::Generic_Bumper(Entity *parent ) + : Controller(parent) +{ + this->myParent = dynamic_cast<ContactSensor*>(this->parent); + + if (!this->myParent) + gzthrow("Bumper controller requires a Contact Sensor as its parent"); +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +Generic_Bumper::~Generic_Bumper() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void Generic_Bumper::LoadChild(XMLConfigNode *node) +{ + this->myIface = dynamic_cast<BumperIface*>(this->ifaces[0]); + + if (!this->myIface) + { + gzthrow("Generic_Bumper controller requires an BumperIface"); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Initialize the controller +void Generic_Bumper::InitChild() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void Generic_Bumper::UpdateChild() +{ + this->myIface->Lock(1); + + this->myIface->data->bumper_count = this->myParent->GetContactCount(); + + for (unsigned int i=0; i < this->myParent->GetContactCount(); i++) + { + this->myIface->data->head.time = this->myParent->GetContactTime(i); + this->myIface->data->bumpers[i] = this->myParent->GetContactState(i); + } + + this->myParent->ResetContactStates(); + + this->myIface->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Finalize the controller +void Generic_Bumper::FiniChild() +{ +} Added: code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.hh =================================================================== --- code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.hh (rev 0) +++ code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.hh 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,86 @@ +/* + * 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: Bumper Controller + * Author: Nate Koenig + * Date: 09 Sept 2008 + */ +#ifndef GENERICBUMPER_CONTROLLER_HH +#define GENERICBUMPER_CONTROLLER_HH + +#include <sys/time.h> + +#include "Controller.hh" +#include "Entity.hh" + +namespace gazebo +{ + class ContactSensor; + + /// \addtogroup gazebo_controller + /// \{ + /** \defgroup bumper bumper + + \brief A controller that returns bump contacts + + \{ + */ + + /// \brief A Bumper controller + class Generic_Bumper : public Controller + { + /// Constructor + public: Generic_Bumper(Entity *parent ); + + /// Destructor + public: virtual ~Generic_Bumper(); + + /// Load the controller + /// \param node XML config node + /// \return 0 on success + protected: virtual void LoadChild(XMLConfigNode *node); + + /// Init the controller + /// \return 0 on success + protected: virtual void InitChild(); + + /// Update the controller + /// \return 0 on success + protected: virtual void UpdateChild(); + + /// Finalize the controller + /// \return 0 on success + protected: virtual void FiniChild(); + + /// The parent Model + private: ContactSensor *myParent; + + /// The Iface. + private: BumperIface *myIface; + }; + + /** \} */ + /// \} + +} + +#endif + Added: code/gazebo/trunk/server/controllers/bumper/generic/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/bumper/generic/SConscript (rev 0) +++ code/gazebo/trunk/server/controllers/bumper/generic/SConscript 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,6 @@ +#Import variable +Import('env sharedObjs') + +sources = Split('Generic_Bumper.cc') + +sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/physics/ContactParams.hh =================================================================== --- code/gazebo/trunk/server/physics/ContactParams.hh 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/server/physics/ContactParams.hh 2008-09-09 22:04:34 UTC (rev 7016) @@ -27,47 +27,60 @@ #ifndef CONTACTPARAMS_HH #define CONTACTPARAMS_HH +#include <boost/signal.hpp> +#include <boost/bind.hpp> + namespace gazebo { + class Geom; + class XMLConfigNode; -/// \brief Contact params -class ContactParams -{ - /// Constructor - public: ContactParams(); - - /// \brief Load the contact params - public: void Load(XMLConfigNode *node); - - /// Spring constant - public: double kp; - - /// Damping constant - public: double kd; - - /// 0..1, 0=no bounciness - public: double bounce; + /// \brief Contact params + class ContactParams + { + /// Constructor + public: ContactParams(); - /// first coefficient of friction - public: double mu1; + /// \brief Load the contact params + public: void Load(XMLConfigNode *node); + + public: template< typename C> + void Callback( void (C::*func)(Geom *, Geom*), C *c ) + { + contactSignal.connect( boost::bind( func, c, _1, _2 ) ); + } + + /// Spring constant + public: double kp; + + /// Damping constant + public: double kd; + + /// 0..1, 0=no bounciness + public: double bounce; + + /// first coefficient of friction + public: double mu1; + + /// second coefficient of friction + public: double mu2; + + /// Force-dependent-slip direction 1 + public: double slip1; + + /// Force-dependent-slip direction 2 + public: double slip2; + + /// \brief bounce vel + public: double bounceVel; + + /// \brief soft constraint force mixing + public: double softCfm; + + public: boost::signal< void (Geom*, Geom*)> contactSignal; + }; +} - /// second coefficient of friction - public: double mu2; - - /// Force-dependent-slip direction 1 - public: double slip1; - - /// Force-dependent-slip direction 2 - public: double slip2; - - /// \brief bounce vel - public: double bounceVel; - - /// \brief soft constraint force mixing - public: double softCfm; -}; - -} #endif Modified: code/gazebo/trunk/server/physics/Geom.cc =================================================================== --- code/gazebo/trunk/server/physics/Geom.cc 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/server/physics/Geom.cc 2008-09-09 22:04:34 UTC (rev 7016) @@ -517,4 +517,3 @@ this->body->UpdateCoM(); } - Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc =================================================================== --- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-09-09 22:04:34 UTC (rev 7016) @@ -287,10 +287,13 @@ contact.surface.slip2 = MIN(geom1->contact->slip2, geom2->contact->slip2); + dJointID c = dJointCreateContact (self->worldId, + self->contactGroup, &contact); + // Call the geom's contact callbacks + geom1->contact->contactSignal( geom1, geom2 ); + geom2->contact->contactSignal( geom2, geom1 ); - dJointID c = dJointCreateContact (self->worldId, - self->contactGroup, &contact); dJointAttach (c,b1,b2); } } Modified: code/gazebo/trunk/server/sensors/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/SConscript 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/server/sensors/SConscript 2008-09-09 22:04:34 UTC (rev 7016) @@ -1,7 +1,7 @@ #Import variable Import('env sharedObjs headers') -dirs = Split('camera ray') +dirs = Split('camera ray contact') for subdir in dirs : SConscript('%s/SConscript' % subdir) @@ -14,5 +14,4 @@ 'server/sensors/Sensor.hh' ] ) -#staticObjs.append( env.StaticObject(sources) ) sharedObjs.append( env.SharedObject(sources) ) Added: code/gazebo/trunk/server/sensors/contact/ContactSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/contact/ContactSensor.cc (rev 0) +++ code/gazebo/trunk/server/sensors/contact/ContactSensor.cc 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,212 @@ +/* + * 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: Contact sensor + * Author: Nate Koenig + * Date: 09 Sept. 2008 + * SVN: $Id:$ +*/ + +#include <assert.h> +#include <float.h> +#include <sstream> + +#include "Global.hh" +//#include "World.hh" +//#include "Controller.hh" + +#include "GazeboError.hh" +#include "XMLConfig.hh" +#include "Simulator.hh" +#include "SensorFactory.hh" +#include "Geom.hh" +#include "ContactParams.hh" +#include "ContactSensor.hh" + +#include "Vector3.hh" + +using namespace gazebo; + +GZ_REGISTER_STATIC_SENSOR("contact", ContactSensor); + +////////////////////////////////////////////////////////////////////////////// +// Constructor +ContactSensor::ContactSensor(Body *body) + : Sensor(body) +{ + this->active = false; + + this->typeName = "contact"; + + this->contactCount = 0; + this->contactStates = NULL; +} + + +////////////////////////////////////////////////////////////////////////////// +// Destructor +ContactSensor::~ContactSensor() +{ + std::vector< ParamT<std::string> *>::iterator iter; + + if (this->contactStates) + delete [] this->contactStates; + + if (this->contactTimes) + delete [] this->contactTimes; + + for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end(); iter++) + { + delete *iter; + } + this->geomNamesP.clear(); +} + +////////////////////////////////////////////////////////////////////////////// +/// Get the contact time +double ContactSensor::GetContactTime(unsigned int index) const +{ + if (index < this->contactCount) + return this->contactTimes[ index ]; + + return 0; +} + +////////////////////////////////////////////////////////////////////////////// +/// Return the number of contacts +unsigned int ContactSensor::GetContactCount() const +{ + return this->contactCount; +} + +////////////////////////////////////////////////////////////////////////////// +/// Return the contact states +uint8_t ContactSensor::GetContactState(unsigned int index) const +{ + if (index < this->contactCount) + return this->contactStates[index]; + + return 0; +} + +////////////////////////////////////////////////////////////////////////////// +/// Reset the contact states +void ContactSensor::ResetContactStates() +{ + memset(this->contactStates, 0, sizeof(uint8_t) * this->contactCount); +} + +////////////////////////////////////////////////////////////////////////////// +/// Load the ray using parameter from an XMLConfig node +void ContactSensor::LoadChild(XMLConfigNode *node) +{ + XMLConfigNode *geomNode = NULL; + + if (this->body == NULL) + { + gzthrow("Null body in the contact sensor"); + } + + Param::Begin(&this->parameters); + geomNode = node->GetChild("geom"); + while (geomNode) + { + ParamT<std::string> *geomName = new ParamT<std::string>("geom","",1); + geomName->SetValue( geomNode->GetValue() ); + this->geomNamesP.push_back(geomName); + geomNode = geomNode->GetNext("geom"); + } + Param::End(); + + this->contactCount = this->geomNamesP.size(); + this->contactTimes = new double[ this->contactCount ]; + this->contactStates = new uint8_t[ this->contactCount ]; + + memset(this->contactStates,0, sizeof(uint8_t) * this->contactCount); + memset(this->contactStates,0, sizeof(double) * this->contactCount); +} + +////////////////////////////////////////////////////////////////////////////// +/// Save the sensor info in XML format +void ContactSensor::SaveChild(std::string &prefix, std::ostream &stream) +{ + std::vector< ParamT<std::string> *>::iterator iter; + + for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end(); iter++) + { + stream << prefix << " " << *(iter) << "\n"; + } +} + +////////////////////////////////////////////////////////////////////////////// +// Init the contact +void ContactSensor::InitChild() +{ + std::vector< ParamT<std::string> *>::iterator iter; + + for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end(); iter++) + { + // Get the geom from the body + Geom *geom = this->body->GetGeom( **(*iter) ); + + // Set the callback + if (geom) + { + geom->contact->Callback( &ContactSensor::ContactCallback, this ); + } + else + { + gzthrow("Unable to find geom[" + **(*iter) + "]"); + } + } + +} + +////////////////////////////////////////////////////////////////////////////// +// Init the ray +void ContactSensor::FiniChild() +{ +} + +////////////////////////////////////////////////////////////////////////////// +// Update the sensor information +void ContactSensor::UpdateChild() +{ +} + +////////////////////////////////////////////////////////////////////////////// +/// Contact callback +void ContactSensor::ContactCallback(Geom *g1, Geom *g2) +{ + std::vector< ParamT<std::string> *>::iterator iter; + int i = 0; + + + for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end(); + iter++, i++) + { + if ( **(*iter) == g1->GetName() || **(*iter) == g2->GetName() ) + { + this->contactStates[i] = 1; + this->contactTimes[i] = Simulator::Instance()->GetRealTime(); + } + } + +} Added: code/gazebo/trunk/server/sensors/contact/ContactSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/contact/ContactSensor.hh (rev 0) +++ code/gazebo/trunk/server/sensors/contact/ContactSensor.hh 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,104 @@ +/* + * 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: Contact sensor + * Author: Nate Koenig + * Date: 09 Sept. 2008 + * SVN: $Id:$ +*/ + +#ifndef CONTACTSENSOR_HH +#define CONTACTSENSOR_HH + +#include <vector> + +#include "Angle.hh" +#include "Sensor.hh" +#include "Body.hh" + +namespace gazebo +{ + + class XMLConfigNode; + class ContactParams; + + /// \addtogroup gazebo_sensor + /// \brief Contact sensor. + /// \{ + /// \defgroup gazebo_ray Contact + /// \brief Contact sensor. + // \{ + + /// \brief Contact sensor. + /// + /// This sensor detects and reports contacts between objects + class ContactSensor: public Sensor + { + /// \brief Constructor + /// \param body The underlying collision test uses an ODE geom, so + /// ray sensors must be attached to a body. + public: ContactSensor(Body *body); + + /// \brief Destructor + public: virtual ~ContactSensor(); + + /// \brief Return the number of contacts + public: unsigned int GetContactCount() const; + + /// \brief Get a contact time + public: double GetContactTime(unsigned int index) const; + + /// \brief Return a contact state + public: uint8_t GetContactState(unsigned int index) const; + + /// \brief Reset the contact states + public: void ResetContactStates(); + + /// Load the contact sensor using parameter from an XMLConfig node + /// \param node The XMLConfig node + protected: virtual void LoadChild(XMLConfigNode *node); + + /// \brief Save the sensor info in XML format + protected: virtual void SaveChild(std::string &prefix,std::ostream &stream); + + /// Initialize the sensor + protected: virtual void InitChild(); + + /// Update sensed values + protected: virtual void UpdateChild(); + + /// Finalize the sensor + protected: virtual void FiniChild(); + + /// \brief Contact callback + private: void ContactCallback(Geom *g1, Geom *g2); + + /// Geom name parameter + private: std::vector< ParamT<std::string> *> geomNamesP; + + private: uint8_t *contactStates; + private: double *contactTimes; + private: unsigned int contactCount; + }; + /// \} + /// \} +} + +#endif Added: code/gazebo/trunk/server/sensors/contact/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/contact/SConscript (rev 0) +++ code/gazebo/trunk/server/sensors/contact/SConscript 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,6 @@ +#Import variable +Import('env sharedObjs') + +sources = Split('ContactSensor.cc') + +sharedObjs.append(env.SharedObject(sources)) Added: code/gazebo/trunk/worlds/bumper.world =================================================================== --- code/gazebo/trunk/worlds/bumper.world (rev 0) +++ code/gazebo/trunk/worlds/bumper.world 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,149 @@ +<?xml version="1.0"?> + +<gazebo:world + xmlns:xi="http://www.w3.org/2001/XInclude" + xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" + xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" + xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" + xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window" + xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param" + xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" + xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" + xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" + xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" + xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui" + xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" + xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" + xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" > + + <verbosity>5</verbosity> + + <physics:ode> + <stepTime>0.03</stepTime> + <gravity>0 0 -9.8</gravity> + <cfm>10e-5</cfm> + <erp>0.8</erp> + <maxUpdateRate>0</maxUpdateRate> + </physics:ode> + + <rendering:gui> + <size>800 600</size> + <pos>100 100</pos> + <frames> + <row height="100%"> + <camera width="100%"> + <xyz>-1 0 4</xyz> + <rpy>0 34 0</rpy> + </camera> + </row> + </frames> + </rendering:gui> + + <rendering:ogre> + <ambient>0.5 0.5 0.5 1.0</ambient> + <sky> + <material>Gazebo/CloudySky</material> + </sky> + + <fog> + <color>1.0 1.0 1.0</color> + <linearStart>10</linearStart> + <linearEnd>100</linearEnd> + </fog> + </rendering:ogre> + + <model:physical name="plane1_model"> + <xyz>0 0 0</xyz> + <rpy>0 0 0</rpy> + <static>true</static> + + <body:plane name="plane1_body"> + <geom:plane name="plane1_geom"> + <normal>0 0 1</normal> + <size>2000 2000</size> + <segments>10 10</segments> + <uvTile>100 100</uvTile> + <material>Gazebo/Grey</material> + </geom:plane> + </body:plane> + </model:physical> + + <model:physical name="sphere1_model"> + <xyz>2.15 -1.68 1.7</xyz> + <rpy>0.0 0.0 0.0</rpy> + <static>false</static> + + <body:sphere name="sphere1_body"> + <geom:sphere name="sphere1_geom"> + <size>0.1</size> + + <visual> + <scale>0.1 0.1 0.1</scale> + <mesh>unit_sphere</mesh> + <material>Gazebo/Rocky</material> + </visual> + </geom:sphere> + + <sensor:contact name="contact_sensor"> + <geom>sphere1_geom</geom> + + <controller:bumper name="bumper_controller"> + <interface:bumper name="bumper_iface" /> + </controller:bumper> + </sensor:contact> + + </body:sphere> + </model:physical> + + <!-- + Include the complete model described in the .model file + This assumes the root node is a <model:...> + --> + <!-- <include embedded="false"> + <xi:include href="pioneer2dx.model" /> + </include> + --> + + <model:physical name="pioneer2dx_model1"> + <xyz>0 0 0.145</xyz> + <rpy>0.0 0.0 0.0</rpy> + + <controller:differential_position2d name="controller1"> + <leftJoint>left_wheel_hinge</leftJoint> + <rightJoint>right_wheel_hinge</rightJoint> + <wheelSeparation>0.39</wheelSeparation> + <wheelDiameter>0.15</wheelDiameter> + <torque>5</torque> + <interface:position name="position_iface_0"/> + </controller:differential_position2d> + + <model:physical name="laser"> + <xyz>0.15 0 0.18</xyz> + + <attach> + <parentBody>chassis_body</parentBody> + <myBody>laser_body</myBody> + </attach> + + <include embedded="true"> + <xi:include href="models/sicklms200.model" /> + </include> + </model:physical> + + <include embedded="true"> + <xi:include href="models/pioneer2dx.model" /> + </include> + </model:physical> + + <!-- White Directional light --> + <model:renderable name="directional_white"> + <light> + <type>directional</type> + <direction>0 -0.6 -0.4</direction> + <diffuseColor>1.0 1.0 1.0</diffuseColor> + <specularColor>0.2 0.2 0.2</specularColor> + <attenuation>1000 1.0 0.0 0</attenuation> + </light> + </model:renderable> + +</gazebo:world> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. ------------------------------------------------------------------------- This SF.Net email is sponsored by the Moblin Your Move Developer's challenge Build the coolest Linux based applications with Moblin SDK & win great prizes Grand prize is a trip for two to an Open Source event anywhere in the world http://moblin-contest.org/redirect.php?banner_id=100&url=/ _______________________________________________ Playerstage-commit mailing list Playerstage-commit@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/playerstage-commit