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
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
