[Robotflow-cvs] RobotFlow/Sensors/src RangeFashionGeneric.cc,NONE,1.1 RangeFashionPioneer2.cc,NONE,1

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Update of /cvsroot/robotflow/RobotFlow/Sensors/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv7521

Modified Files:
	Makefile.am 
Added Files:
	RangeFashionGeneric.cc RangeFashionPioneer2.cc 
Log Message:

Index: Makefile.am
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Sensors/src/Makefile.am,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** Makefile.am	27 Oct 2005 18:55:17 -0000	1.2
--- Makefile.am	20 Jan 2006 22:00:56 -0000	1.3
***************
*** 7,11 ****

  # Sources for compilation in the library
! libRFSensors_la_SOURCES = RangeFashion.cc

  libRFSensors_la_LDFLAGS = -release $(LT_RELEASE)
--- 7,13 ----

  # Sources for compilation in the library
! libRFSensors_la_SOURCES = RangeFashion.cc \
!                      RangeFashionPioneer2.cc \
!                      RangeFashionGeneric.cc

  libRFSensors_la_LDFLAGS = -release $(LT_RELEASE)

--- NEW FILE: RangeFashionPioneer2.cc ---
/* Copyright (C) 2005 Patrick Frenette (Pat...@US...)

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.

   This library 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
   Lesser General Public License for more details.

   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/
#ifndef _RANGE_FASHION_CC_
#define _RANGE_FASHION_CC_

#include "BufferedNode.h"
#include "Vector.h"

using namespace std;
using namespace FD;

namespace RobotFlow {

class RangeFashionPioneer2;

DECLARE_NODE(RangeFashionPioneer2)

/*Node
 *
 * @name RangeFashionPioneer2
 * @category RobotFlow:Sensors
 * @description Fusion range device in a unique percept
 *
 * @input_name SONAR
 * @input_type Vector<int>
 * @input_description All sonars reading.
 *
 * @output_name RANGE_BELT
 * @output_type Vector<int>
 * @output_description Percept after fusion
 *
 * @parameter_name TYPE_FASHION
 * @parameter_description  Determine of to make the sensor fashion
 * @parameter_type int
 * @parameter_value 0
 *
 * @parameter_name MAX_DIST
 * @parameter_description  Distance when no data from sensor (mm)
 * @parameter_type int
 * @parameter_value 10000
 *
 * @parameter_name MAX_DIST_SONAR
 * @parameter_description  Max range detection of sonar (mm)
 * @parameter_type int
 * @parameter_value 10000 
 *
END*/

class RangeFashionPioneer2 : public BufferedNode {

  //inputs
  int sonarID;

  //outputs
  int perceptID;

  //parameters
  int m_typeFashion;
  int m_maxDist;
  int m_maxDistSonar;

public:

   RangeFashionPioneer2(string nodeName, ParameterSet params)
   : BufferedNode(nodeName, params)  {

     //inputs
     sonarID = addInput("SONAR");

     //outputs
     perceptID = addOutput("RANGE_BELT");

     // parameters
     m_typeFashion  = dereference_cast<int>(parameters.get("TYPE_FASHION"));
     m_maxDist      = dereference_cast<int>(parameters.get("MAX_DIST"));
     m_maxDistSonar = dereference_cast<int>(parameters.get("MAX_DIST_SONAR"));
   }

   void calculate(int output_id, int count, Buffer &out)
   {
     ObjectRef sonarValue = getInput(sonarID,count);

     Vector<int> *vectPercept = Vector<int>::alloc(12);

     switch (m_typeFashion)
     {
        case 0:
            vectPercept->assign(12, -1);
            if(!sonarValue->isNil()){
               Vector<float> &sonarReading = object_cast<Vector<float> >(sonarValue);

               for (int i = 0; i < 12; i++){
                  //front
                  if (i < 7){
                     if ((*vectPercept)[i] == -1){
                        if (sonarReading[6-i] < sonarReading[7-i] && sonarReading[6-i] < m_maxDistSonar){
                           (*vectPercept)[i] = (int)sonarReading[6-i];
                        }
                        else if (sonarReading[7-i] < m_maxDistSonar){
                           (*vectPercept)[i] = (int)sonarReading[7-i];
                        }
                        else {
                           (*vectPercept)[i] = m_maxDistSonar;
                        }
                     }
                  }
                  //back avoided have problem
                  /*else if (i == 7){//back
                     if ((*vectPercept)[i] == -1){
                        if (sonarReading[14] < sonarReading[15] && sonarReading[14] < m_maxDistSonar){
                           (*vectPercept)[i] = (int)sonarReading[14];
                        }
                        else if (sonarReading[15] < m_maxDistSonar){
                           (*vectPercept)[i] = (int)sonarReading[15];
                        }
                        else {
                           (*vectPercept)[i] = m_maxDistSonar;
                        }
                     }
                  }
                  else if (i < 11){//back
                     if ((*vectPercept)[i] == -1){
                        if (sonarReading[20-i] < sonarReading[21-i] && sonarReading[20-i] < m_maxDistSonar){
                           (*vectPercept)[i] = (int)sonarReading[20-i];
                        }
                        else if (sonarReading[21-i] < m_maxDistSonar){
                           (*vectPercept)[i] = (int)sonarReading[21-i];
                        }
                        else {
                           (*vectPercept)[i] = m_maxDistSonar;
                        }
                     }
                  }
                  else if (i == 11){//back
                     if ((*vectPercept)[i] == -1){
                        if (sonarReading[8] < sonarReading[9] && sonarReading[8] < m_maxDistSonar){
                           (*vectPercept)[i] = (int)sonarReading[8];
                        }
                        else if (sonarReading[9] < m_maxDistSonar){
                           (*vectPercept)[i] = (int)sonarReading[9];
                        }
                        else {
                           (*vectPercept)[i] = m_maxDistSonar;
                        }
                     }
                  }*/
               }
            }
            // Substitue max range for the value of -1
            Vector<int>::iterator iterPercept;
            for(iterPercept = vectPercept->begin(); iterPercept != vectPercept->end(); ++iterPercept) {
               if(*iterPercept < 0)  
                  *iterPercept = m_maxDist;
            }

            break;
     };

     out[count] = ObjectRef(vectPercept); 
     cerr << "RangeFashion" << endl;
   }//calculate
};

}//namespace RobotFlow
#endif

--- NEW FILE: RangeFashionGeneric.cc ---
/* Copyright (C) 2005 Patrick Frenette (Pat...@US...)

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.

   This library 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
   Lesser General Public License for more details.

   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/
#ifndef _RANGE_FASHION_CC_
#define _RANGE_FASHION_CC_

#include "BufferedNode.h"
#include "Vector.h"

using namespace std;
using namespace FD;

namespace RobotFlow {

class RangeFashionGeneric;

DECLARE_NODE(RangeFashionGeneric)

/*Node
 *
 * @name RangeFashionGeneric
 * @category RobotFlow:Sensors
 * @description Fusion range device in a unique percept
 *
 * @input_name GENERIC
 * @input_type Vector<ObjectRef>
 * @input_description ObjectRef is a vector<float> which contains a distance and a angle
 *
 * @output_name RANGE_BELT
 * @output_type Vector<int>
 * @output_description Percept after fusion
 *
 * @parameter_name TYPE_FASHION
 * @parameter_description  Determine of to make the sensor fashion
 * @parameter_type int
 * @parameter_value 0
 *
 * @parameter_name BELT_NUMBER
 * @parameter_description  Determine the precision 
 * @parameter_type int
 * @parameter_value 12
 *
 * @parameter_name MAX_DIST
 * @parameter_description  Distance when no data from sensor (mm)
 * @parameter_type int
 * @parameter_value 10000
 *
END*/

class RangeFashionGeneric : public BufferedNode {

  //inputs
  int genericID;

  //outputs
  int perceptID;

  //parameters
  int m_typeFashion;
  int m_maxDist;
  int m_beltNbr;

public:

   RangeFashionGeneric(string nodeName, ParameterSet params)
   : BufferedNode(nodeName, params)  {

     //inputs
     genericID = addInput("GENERIC");

     //outputs
     perceptID = addOutput("RANGE_BELT");

     // parameters
     m_typeFashion  = dereference_cast<int>(parameters.get("TYPE_FASHION"));
     m_maxDist      = dereference_cast<int>(parameters.get("MAX_DIST"));
     m_beltNbr      = dereference_cast<int>(parameters.get("BELT_NUMBER"));
   }

   void calculate(int output_id, int count, Buffer &out)
   {
      ObjectRef genericValue = getInput(genericID,count);

      Vector<int> *vectPercept = Vector<int>::alloc(m_beltNbr);

      float rangeBelt = 3600 / m_beltNbr;

      switch (m_typeFashion)
      {
         case 0: 
            // Init vectPercept to -1
            vectPercept->assign(12, -1);

            //Distance & Theta
            if (!genericValue->isNil()){
               Vector<ObjectRef> &genericVector = object_cast<Vector<ObjectRef> >(genericValue);

               for (int i = 0; i < genericVector.vsize(); i++){
                  Vector<float> &obstacleInfo = object_cast<Vector<float> >(genericVector[i]);
                  if (obstacleInfo[0] < m_maxDist){
                     if ( obstacleInfo[1] < (rangeBelt/2) || obstacleInfo[1] > (3600 - (rangeBelt/2)) ){
                        if ((*vectPercept)[0] == -1 || (int)obstacleInfo[0] < (*vectPercept)[0])
                        (*vectPercept)[0] = (int)obstacleInfo[0];
                     }
                     else{
                        int index = ((int)((obstacleInfo[1] - rangeBelt) / m_beltNbr)) + 1;
                        (*vectPercept)[index] = (int)obstacleInfo[0];
                     }
                  }
               }
            }
            break;
     }

     out[count] = ObjectRef(vectPercept); 
     cerr << "RangeFashion" << endl;
   }//calculate
};

}//namespace RobotFlow
#endif

[Robotflow-cvs] RobotFlow/Sensors/src RangeFashionGeneric.cc,NONE,1.1 RangeFashionPioneer2.cc,NONE,1

[Robotflow-cvs] RobotFlow/Sensors/src RangeFashionGeneric.cc,NONE,1.1 RangeFashionPioneer2.cc,NONE,1.1 Makefile.am,1.2,1.3