Robotsystem from Robot Group Leipzig: raysensorbank.cpp Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# raysensorbank.cpp

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius@informatik.uni-leipzig.de                                    *
    00004  *    fhesse@informatik.uni-leipzig.de                                     *
    00005  *    der@informatik.uni-leipzig.de                                        *
    00006  *                                                                         *
    00007  *   This program is free software; you can redistribute it and/or modify  *
    00008  *   it under the terms of the GNU General Public License as published by  *
    00009  *   the Free Software Foundation; either version 2 of the License, or     *
    00010  *   (at your option) any later version.                                   *
    00011  *                                                                         *
    00012  *   This program is distributed in the hope that it will be useful,       *
    00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00015  *   GNU General Public License for more details.                          *
    00016  *                                                                         *
    00017  *   You should have received a copy of the GNU General Public License     *
    00018  *   along with this program; if not, write to the                         *
    00019  *   Free Software Foundation, Inc.,                                       *
    00020  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00021  *                                                                         *
    00022  *   $Log: raysensorbank.cpp,v $
    00023  *   Revision 1.2.4.4  2006/01/12 15:14:57  martius
    00024  *   indentation and clear routine
    00025  *
    00026  *   Revision 1.2.4.3  2005/12/14 15:37:19  martius
    00027  *   sensors are working with osg
    00028  *
    00029  *   Revision 1.2.4.2  2005/12/14 12:43:07  martius
    00030  *   moved to osg
    00031  *
    00032  *   Revision 1.2.4.1  2005/12/13 18:11:53  martius
    00033  *   sensors ported, but not yet finished
    00034  *
    00035  *   Revision 1.2  2005/09/27 13:59:26  martius
    00036  *   ir sensors are working now
    00037  *
    00038  *   Revision 1.1  2005/09/27 11:03:34  fhesse
    00039  *   sensorbank added
    00040  *
    00041  *                                                                         *
    00042  ***************************************************************************/
    00043
    00044 #include <assert.h>
    00045 #include <ode/ode.h>
    00046 #include <osg/Matrix>
    00047
    00048 #include "raysensorbank.h"
    00049
    00050 using namespace osg;
    00051
    00052 namespace lpzrobots {
    00053
    00054   RaySensorBank::RaySensorBank(){
    00055     initialized=false;
    00056   };
    00057
    00058   RaySensorBank::~RaySensorBank(){
    00059     clear();
    00060   };
    00061
    00062   void RaySensorBank::init(const OdeHandle& odeHandle, const OsgHandle& osgHandle){
    00063     this->odeHandle = odeHandle;
    00064     this->osgHandle = osgHandle;
    00065     this->odeHandle.space = dSimpleSpaceCreate ( this->odeHandle.space );
    00066     initialized=true;
    00067   };
    00068
    00069   unsigned int RaySensorBank::registerSensor(RaySensor* raysensor, Primitive* body,
    00070                                              const Matrix& pose, double range,
    00071                                              RaySensor::rayDrawMode drawMode){
    00072     raysensor->init(odeHandle, osgHandle, body, pose, range, drawMode);
    00073     bank.push_back(raysensor);
    00074     return bank.size();
    00075   };
    00076
    00077   void RaySensorBank::reset(){
    00078     for (unsigned int i=0; i<bank.size(); i++){
    00079       bank[i]->reset();
    00080     }
    00081   };
    00082
    00083   bool RaySensorBank::sense(dGeomID object){
    00084     bool sth_sensed=false;
    00085     for (unsigned int i=0; i<bank.size(); i++){
    00086       if (bank[i]->sense(object)){
    00087         sth_sensed=true;
    00088       }
    00089     }
    00090     return sth_sensed;
    00091   };
    00092
    00093   double RaySensorBank::get(unsigned int index){
    00094     assert(index<bank.size());
    00095     return bank[index]->get();
    00096   };
    00097
    00098   int RaySensorBank::get(double* sensorarray, unsigned int array_size){
    00099     int counter=0;
    00100     for(unsigned int i=0; (i<array_size) && (i<bank.size()); i++){
    00101       sensorarray[i]=bank[i]->get();
    00102       counter++;
    00103     }
    00104     return counter;
    00105   };
    00106
    00107   dSpaceID RaySensorBank::getSpaceID(){
    00108     return odeHandle.space;
    00109   };
    00110
    00111   void RaySensorBank::update(){
    00112     for (unsigned int i=0; i<bank.size(); i++){
    00113       bank[i]->update();
    00114     }
    00115   };
    00116
    00117   // delete all registered sensors.
    00118   void RaySensorBank::clear(){
    00119     for (unsigned int i=0; i<bank.size(); i++){
    00120       if(bank[i]) delete bank[i];
    00121     }
    00122     bank.clear();
    00123   }
    00124
    00125
    00126 }
    00127
    00128
    00129
    00130
    00131