We have created a sensor representation of a BoxGeom in Gazebo and we'd like to
test when this box collides with RayGeoms which are being emitted from a laser
on another robot. Currently the only collisions we're getting are between
class types 7&7 or 4&7 which are GeomTransforms and GeomPlanes. Any ideas on
what we could be doing wrong? I've attached our code. Because we've been
trying to many things, some parts may be commented out. We tried to base our
sensor off the gazebo ray sensor.
Thanks.
~Lexi
/*
* 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: Ear proximity sensor
* Author: Carle Cote
* Date: 23 february 2004
* SVN: $Id: EarSensor.cc 6965 2008-08-20 21:53:33Z natepak $
*/
#include <assert.h>
#include <float.h>
#include <sstream>
#include "SensorFactory.hh"
#include "XMLConfig.hh"
#include "Global.hh"
#include "RayGeom.hh"
#include "World.hh"
#include "PhysicsEngine.hh"
#include "GazeboError.hh"
#include "ODEPhysics.hh"
#include "XMLConfig.hh"
#include "Controller.hh"
#include "EarSensor.hh"
#include "BoxGeom.hh"
#include "Vector3.hh"
using namespace gazebo;
GZ_REGISTER_STATIC_SENSOR("ear", EarSensor);
//////////////////////////////////////////////////////////////////////////////
// Constructor
EarSensor::EarSensor(Body *body)
: Sensor(body)
{
this->active = false;
this->typeName = "ear";
this->rangeCountP = new Param<int>("rangeCount",0,1);
this->minRangeP = new Param<double>("minRange",0,1);
this->maxRangeP = new Param<double>("maxRange",0,1);
this->originP = new Param<Vector3>("origin", Vector3(0,0,0), 0);
}
//////////////////////////////////////////////////////////////////////////////
// Destructor
EarSensor::~EarSensor()
{
delete this->rangeCountP;
delete this->minRangeP;
delete this->maxRangeP;
delete this->originP;
}
//////////////////////////////////////////////////////////////////////////////
/// Load the ray using parameter from an XMLConfig node
void EarSensor::LoadChild(XMLConfigNode *node)
{
if (this->body == NULL) {
gzthrow("Null body in the ear sensor");
}
this->rangeCountP->Load(node);
this->minRangeP->Load(node);
this->maxRangeP->Load(node);
this->originP->Load(node);
// Create a space to contain the ray space
this->superSpaceId = dSimpleSpaceCreate( 0 );
// Create a space to contain all the rays
this->raySpaceId = dSimpleSpaceCreate( this->superSpaceId );
//this->raySpaceId = dSimpleSpaceCreate( 0 );
//this->superSpaceId = dSimpleSpaceCreate( this->raySpaceId );
// Set collision bits
//dGeomSetCategoryBits((dGeomID) this->superSpaceId, GZ_ALL_COLLIDE);
//GZ_ALL_COLLIDE
//dGeomSetCollideBits((dGeomID) this->superSpaceId, GZ_ALL_COLLIDE);
dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_ALL_COLLIDE);
//GZ_ALL_COLLIDE
dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_ALL_COLLIDE);
this->body->spaceId = this->raySpaceId;
//this->body->spaceId = this->superSpaceId;
}
//////////////////////////////////////////////////////////////////////////////
/// Save the sensor info in XML format
void EarSensor::SaveChild(std::string &prefix, std::ostream &stream)
{
stream << prefix << " " << *(this->minRangeP) << "\n";
stream << prefix << " " << *(this->maxRangeP) << "\n";
stream << prefix << " " << *(this->originP) << "\n";
stream << prefix << " " << *(this->rangeCountP) << "\n";
}
//////////////////////////////////////////////////////////////////////////////
// Init the ray
void EarSensor::InitChild()
{
Pose3d bodyPose;
double angle;
Vector3 start, end, axis,size;
//RayGeom *ray;
bodyPose = this->body->GetPose();
this->prevPose = bodyPose;
//Don't want to send rays out into the environment; just want to detect
when a ray hits location originP
//sensorRep = dCreateBox(dSpaceID space, dReal lx, dReal ly, dReal lz);
sensorRep = new BoxGeom(this->body);
size.Set(1,1,1);
sensorRep->SetSize(size);
//sensorRep->SetCategoryBits( GZ_ALL_COLLIDE );
//sensorRep->SetCollideBits(~GZ_ALL_COLLIDE );
//this->body->AttachGeom(sensorRep);
}
//////////////////////////////////////////////////////////////////////////////
// Init the ray
void EarSensor::FiniChild()
{
}
//////////////////////////////////////////////////////////////////////////////
/// Get the minimum range
double EarSensor::GetMinRange() const
{
return this->minRangeP->GetValue();
}
//////////////////////////////////////////////////////////////////////////////
/// Get the maximum range
double EarSensor::GetMaxRange() const
{
return this->maxRangeP->GetValue();
}
//////////////////////////////////////////////////////////////////////////////
/// Get the range count
int EarSensor::GetRangeCount() const
{
//return this->rangeCountP->GetValue();
return this->ranges.size();
}
//////////////////////////////////////////////////////////////////////////////
// Get detected range for a ray
double EarSensor::GetRange(int index)
{
if (index < 0 || index >= (int)this->ranges.size()) {
std::ostringstream stream;
stream << "index[" << index << "] out of range[0-" <<
this->ranges.size() << "]";
gzthrow(stream.str());
}
return this->ranges[index];
}
//////////////////////////////////////////////////////////////////////////////
// Get detected retro (intensity) value for a ray.
double EarSensor::GetRetro(int index)
{
if (index < 0 || index >= (int)this->intensity.size()) {
std::ostringstream stream;
stream << "index[" << index << "] out of range[0-" <<
this->intensity.size() << "]";
gzthrow(stream.str());
}
return this->intensity[index];
}
//////////////////////////////////////////////////////////////////////////////
// Update the sensor information
void EarSensor::UpdateChild()
{
Pose3d poseDelta;
Vector3 a, b;
// Get the pose of the sensor body (global cs)
poseDelta = this->body->GetPose() - this->prevPose;
this->prevPose = this->body->GetPose();
ODEPhysics *ode =
dynamic_cast<ODEPhysics*>(World::Instance()->GetPhysicsEngine());
if (ode == NULL) {
gzthrow( "Invalid physics engine. Must use ODE." );
}
// Do collision detection
//std::cout<<(( dGeomID )this->raySpaceId)<<" "<<( dGeomID
)this->superSpaceId<<" "<<( dGeomID )ode->GetSpaceId()<<std::endl;
dSpaceCollide2( ( dGeomID ) ( this->superSpaceId ),
( dGeomID ) ( ode->GetSpaceId() ),
this, &UpdateCallback );
// }
}
/////////////////////////////////////////////////////////////////////////////
// Callback for ray intersection test
void EarSensor::UpdateCallback( void *data, dGeomID o1, dGeomID o2 )
{
int n = 0;
dContactGeom contact;
dxGeom *geom1, *geom2 = NULL;
RayGeom *rayGeom = NULL;
Geom *hitGeom = NULL;
EarSensor *self = (EarSensor*) data;
const double delta = 0.2;
/**********************************
% dSphereClass 0 Sphere
% dBoxClass 1 Box
% dCapsuleClass 2 Capsule (i.e. cylinder with half-sphere
caps at its ends)
% dCylinderClass 3 Regular flag ended Cylinder
% dPlaneClass 4 Infinite plane (non-placeable)
% dRayClass 5 Ray
% dConvexClass 6
% dGeomTransformClass 7 Geometry transform
% dTrimeshClass 8 Triangle mesh
**********************************/
// Check space
if ( dGeomIsSpace( o1 ) || dGeomIsSpace( o2 ) ) {
if (dGeomGetSpace(o1) == self->superSpaceId ||
dGeomGetSpace(o2) == self->superSpaceId) {
dSpaceCollide2( o1, o2, self, &UpdateCallback );
}
if (dGeomGetSpace(o1) == self->raySpaceId || dGeomGetSpace(o2)
== self->raySpaceId) {
dSpaceCollide2( o1, o2, self, &UpdateCallback );
}
}
else {
geom1 = NULL;
geom2 = NULL;
// Get pointers to the underlying geoms
if (dGeomGetClass(o1) == dGeomTransformClass)
geom1 = (dxGeom*)
dGeomGetData(dGeomTransformGetGeom(o1));
else
geom1 = (dxGeom*) dGeomGetData(o1);
if (dGeomGetClass(o2) == dGeomTransformClass)
geom2 = (dxGeom*)
dGeomGetData(dGeomTransformGetGeom(o2));
else
geom2 = (dxGeom*) dGeomGetData(o2);
assert(geom1 && geom2);
std::cout<<"dgeomgetclass = "<<dGeomGetClass(o1)<<'
'<<dGeomGetClass(o2)<<std::endl;
rayGeom = NULL;
hitGeom = NULL;
// Figure out which one is a ray; note that this assumes
// that the ODE dRayClass is used *soley* by the RayGeom.
if (dGeomGetClass(o1) == dRayClass) {
rayGeom = (RayGeom*) geom1;
hitGeom = (Geom*) geom2;
dGeomRaySetParams(o1, 0, 0);
dGeomRaySetClosestHit(o1, 1);
}
if (dGeomGetClass(o2) == dRayClass) {
assert(rayGeom == NULL);
rayGeom = (RayGeom*) geom2;
hitGeom = (Geom* )geom1;
dGeomRaySetParams(o2, 0, 0);
dGeomRaySetClosestHit(o2, 1);
}
//std::cout<<"geomclass = "<<dGeomGetClass(o1)<<'
'<<dGeomGetClass(o2)<<' '<<dRayClass<<' '<<dBoxClass<<std::endl;
// Check for ray/geom intersections
if ( rayGeom && hitGeom ) {
n = dCollide(o1, o2, 1, &contact, sizeof(contact));
std::cout<<"dcollide = "<<n<<std::endl;
if ( n > 0 ) {
//if (contact.depth < rayGeom->GetLength())
//{
//rayGeom->SetLength( contact.depth );
//rayGeom->SetRetro( hitGeom->GetLaserRetro() );
int Hit=true;
double p;
for(int i=0;i<2;i++) {
p = self->originP->GetValue()[i];
if(fabs(contact.pos[i]-p)>delta) {
Hit=false;
break;
}
}
std::cout<<"Hit: "<<Hit<<std::endl;
if (Hit) {
self->AddSound(contact.depth,hitGeom->GetLaserRetro());
//rayGeom->SetFiducial(
hitGeom->GetLaserFiducialId() );
}
}
}
}
}
void EarSensor::AddSound(double newrange, double newinten)
{
this->ranges.push_back(newrange);
this->intensity.push_back(newinten);
}
-------------------------------------------------------------------------
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-gazebo mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-gazebo
