[Robotflow-cvs] RobotFlow/Sensors/src Makefile.am,NONE,1.1 RangeFashion.cc,NONE,1.1

SourceForge Headquarters 225 Broadway Suite 1600 San Diego, CA 92101 +1 (858) 422-6466

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

Added Files:
	Makefile.am RangeFashion.cc 
Log Message:
Moved from Behaviors, created new directory Sensors

--- NEW FILE: Makefile.am ---
## Process this file with automake to produce Makefile.in. -*-Makefile-*-
# $Id: Makefile.am,v 1.1 2005/10/27 18:02:18 maestro Exp $
# Disable automatic dependency tracking if using other tools than gcc and gmake
#AUTOMAKE_OPTIONS = no-dependencies

lib_LTLIBRARIES = libRFSensors.la

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

libRFSensors_la_LDFLAGS = -release $(LT_RELEASE)

INCLUDES = -I../include $(OVERFLOW_INCLUDE)

install-data-local:
	echo "Installing libRFSensors in $(libdir)"
	mkdir -p $(libdir)
	(perl $(OVERFLOW_BIN)/info2def.pl $(libRFSensors_la_SOURCES) > $(datadir)/RFSensors.def)
	(rm -f $(libdir)/RFSensors.tlb; cd $(libdir); ln -s libRFSensors.so RFSensors.tlb)

--- NEW FILE: RangeFashion.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 RangeFashion;

DECLARE_NODE(RangeFashion)

/*Node
 *
 * @name RangeFashion
 * @category RobotFlow:Sensors
 * @description Fusion range device in a unique percept
 *
 * @input_name LASER
 * @input_type Vector<int>
 * @input_description All lasers reading.
 *
 * @input_name SONAR
 * @input_type Vector<int>
 * @input_description All sonars reading.
 *
 * @input_name IR
 * @input_type Vector<int>
 * @input_description All IRs 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_IR
 * @parameter_description  Max range detection of IR (mm)
 * @parameter_type int
 * @parameter_value 1000
 *
 * @parameter_name MAX_DIST_SONAR
 * @parameter_description  Max range detection of sonar (mm)
 * @parameter_type int
 * @parameter_value 10000 
 *
 * @parameter_name MAX_DIST_LASER
 * @parameter_description  Max range detection of laser (mm)
 * @parameter_type int
 * @parameter_value 10000 
 *
END*/

class RangeFashion : public BufferedNode {

  //inputs
  int laserID;
  int sonarID;
  int irID;

  //outputs
  int perceptID;

  //parameters
  int m_typeFashion;
  int m_maxDist;
  int m_maxDistIR;
  int m_maxDistSonar;
  int m_maxDistLaser;

  void priorityBaseFashion(Vector<int> *vectPercept, ObjectRef &laserValue, ObjectRef &sonarValue, ObjectRef &irValue, int count){

      // Init vectPercept to -1
      vectPercept->assign(12, -1);

      // Priority #1 
      // IRs
      if(!irValue->isNil()){
          Vector<int> &irReading = object_cast<Vector<int> >(getInput(irID,count));

          // Copy IR values 
          Vector<int>::iterator iterIR;
          int i=0;
          for(iterIR = irReading.begin(); iterIR != irReading.end(); ++iterIR){
              if(*iterIR <= m_maxDistIR)  // Max value IR
                  (*vectPercept)[i]=*iterIR;
	      ++i;
          }		
      }

      /*Vector<int>::iterator iterA;
      std::cout << "IR -> ";
      for(iterA = vectPercept->begin(); iterA != vectPercept->end(); ++iterA){
          std::cout << *iterA << " ";
      }
      std::cout << std::endl;*/

      // Priority #2
      // Lasers
      if(!laserValue->isNil()){
          Vector<int> &laserReading = object_cast<Vector<int> >(getInput(laserID,count));

          int ind_vectPercept = 3;

          // For each position in the range of the laser
          for(int i=0; i<7; ++i){
              // Determine the range for the position
              int begin = 60 * i - 10;
              int end = 60 * i + 10;
              if (i==0) begin = 0;
              else if (i==6) end = 360;

              // Get the 3 mins of the range
              int min1=m_maxDistLaser, min2=m_maxDistLaser, min3=m_maxDistLaser;
              for(int j=begin; j<end; ++j){
                  if(laserReading[j] <= min1){
                      min3 = min2;
                      min2 = min1;
                      min1 = laserReading[j];
                  }
                  else if(laserReading[j] <= min2){
                      min3 = min2;
                      min2 = laserReading[j];
                  }
                  else if(laserReading[j] <= min3){
                      min3 = laserReading[j];
                  }
              }

              // Evaluate the mean of the min
              int x_min = (min1+min2+min3)/3;

	      // Transform of the data according to the physical dimension of the robot
	      int offset = 0;
	      /*switch (ind_vectPercept){
            	case 0: offset = 200; break;
            	case 1: offset = 150; break;
            	case 2: offset = 150; break;
            	case 3: offset = 300; break;
            	case 9: offset = 300; break;
            	case 10: offset = 150; break;
            	case 11: offset = 150; break;
	      }*/

          // Save the value
          if(((*vectPercept)[ind_vectPercept] == -1) && (x_min < m_maxDistLaser)){	      
             (*vectPercept)[ind_vectPercept] = x_min - offset;
	  }

          // Next position
          --ind_vectPercept;
          if(ind_vectPercept<0)
             ind_vectPercept = 11;
          }
      }

      /*std::cout << "Laser -> ";
      for(iterA = vectPercept->begin(); iterA != vectPercept->end(); ++iterA){
          std::cout << *iterA << " ";
      }
      std::cout << std::endl;*/

      // Priority #3
      // Sonars
      if(!sonarValue->isNil()){
          Vector<int> &sonarReading = object_cast<Vector<int> >(getInput(sonarID,count));

          // Copy Sonar values 
          Vector<int>::iterator iterSonar;
          int i=0;
          for(iterSonar = sonarReading.begin(); iterSonar != sonarReading.end(); ++iterSonar) {
              if(((*vectPercept)[i] == -1) && (*iterSonar<m_maxDistSonar))  // Max value Sonar
                  (*vectPercept)[i]=*iterSonar;
              ++i;
          }		
      }

      /*std::cout << "Sonar -> ";
      for(iterA = vectPercept->begin(); iterA != vectPercept->end(); ++iterA){
          std::cout << *iterA << " ";
	  }
	  std::cout << std::endl;*/

      // 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;
      }

      /*std::cout << "Percept -> ";
      for(iterA = vectPercept->begin(); iterA != vectPercept->end(); ++iterA){
          std::cout << *iterA << " ";
	  }
	  std::cout << std::endl;*/

  }

public:

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

     //inputs
     laserID = addInput("LASER");
     sonarID = addInput("SONAR");
     irID = addInput("IR");

     //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_maxDistIR    = dereference_cast<int>(parameters.get("MAX_DIST_IR"));
     m_maxDistSonar = dereference_cast<int>(parameters.get("MAX_DIST_SONAR"));
     m_maxDistLaser = dereference_cast<int>(parameters.get("MAX_DIST_LASER"));
   }

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

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

     switch (m_typeFashion)
     {
        case 0: priorityBaseFashion(vectPercept, laserValue, sonarValue, irValue, count);
                percept = ObjectRef(vectPercept);
                break;
     }

     out[count] = percept; 
   }//calculate

};

}//namespace RobotFlow
#endif