[Robotflow-cvs] RobotFlow/Behaviors/src RangeFashion.cc,NONE,1.1

[Fernyqc <fe...@us...>]

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

Added Files:
	RangeFashion.cc 
Log Message:
First version of the file


--- NEW FILE: RangeFashion.cc ---
/* Copyright (C) 2002 Dominic Letourneau (dom...@co...)

   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 "Behavior.h"
#include "Vector.h"

using namespace std;
using namespace FD;

namespace RobotFlow {

class RangeFashion;

DECLARE_NODE(RangeFashion)
REGISTER_BEHAVIOR(RangeFashion)

/*Node

 * @name RangeFashion
 * @category RobotFlow:Behaviors
 * @description Fusion range device in a unique percept
 *
 * @input_name ACTIVATED
 * @input_type bool
 * @input_description Behavior activation
 *
 * @input_name LASERS
 * @input_type Vector<int>
 * @input_description All lasers reading.
 *
 * @input_name SONARS
 * @input_type Vector<int>
 * @input_description All sonars reading.
 *
 * @input_name IRS
 * @input_type Vector<int>
 * @input_description All IRs reading.
 *
 * @output_name PERCEPT
 * @output_type Vector<int>
 * @output_description Percept after fusion
 *
 * @output_name EXPLOITATION
 * @output_description Behavior exploitation
 *
 * @parameter_name TYPE_FASHION
 * @parameter_description  Determine of to make the sensor fashion
 * @parameter_type int
 * @parameter_value 0
 *
END*/


class RangeFashion : public Behavior {
   
  //inputs
  int laserID;
  int sonarID;
  int irID;

  //outputs
  int perceptID;

  //parameters
  int m_typeFashion;
  
  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 <= 100)  // Max value IR
                  (*vectPercept)[i]=*iterIR;
              ++i;
          }		
      }
      
      // 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=10000, min2=10000, min3=10000;
              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;
              
              // Save the value
              if(((*vectPercept)[ind_vectPercept] == -1) && (x_min < 10000))
                  (*vectPercept)[ind_vectPercept] = x_min;
              
              // Next position
              --ind_vectPercept;
              if(ind_vectPercept<0)
                  ind_vectPercept = 11;
          }
          
      }
      
      // 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((*iterSonar = -1) && (*iterSonar<10000))  // Max value Sonar
                  (*vectPercept)[i]=*iterSonar;
              ++i;
          }		
      }
      
      // Substitue max range for the value of -1
      Vector<int>::iterator iterPercept;
      for(iterPercept = vectPercept->begin(); iterPercept != vectPercept->end(); ++iterPercept) {
        if(*iterPercept < 0)  // Max value IR
            *iterPercept = 10000;
      }
  }

public:

   RangeFashion(string nodeName, ParameterSet params)
   : Behavior(nodeName, params, "RangeFashion")  {

     //inputs
     laserID = addInput("LASERS");
     sonarID = addInput("SONARS");
     irID = addInput("IRS");

     //outputs
     perceptID = addOutput("PERCEPT");
     
     // parameters
     m_typeFashion = dereference_cast<int>(parameters.get("TYPE_FASHION"));
   }

   void calculate_behavior(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_behavior
  
};

}//namespace RobotFlow
#endif