[Robotflow-cvs] RobotFlow/Behaviors/src AvoidUtil.cc,NONE,1.1 EllipticalAvoid.cc,NONE,1.1 AvoidPat.c

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

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

Modified Files:
	AvoidPat.cc Makefile.am 
Added Files:
	AvoidUtil.cc EllipticalAvoid.cc 
Log Message:
Reffactoring of AvoidPat to EllipticalAvoid

Index: Makefile.am
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/Makefile.am,v
retrieving revision 1.20
retrieving revision 1.21
diff -C2 -d -r1.20 -r1.21
*** Makefile.am	27 Oct 2005 18:04:25 -0000	1.20
--- Makefile.am	12 Dec 2005 18:27:49 -0000	1.21
***************
*** 35,39 ****
  	SafeVelocityPat.cc \
  	AvoidPat.cc \
! 	BumperStall.cc

  libBehaviors_la_LDFLAGS = -release $(LT_RELEASE)
--- 35,41 ----
  	SafeVelocityPat.cc \
  	AvoidPat.cc \
!    EllipticalAvoid.cc \
!    AvoidUtil.cc \
!    BumperStall.cc

  libBehaviors_la_LDFLAGS = -release $(LT_RELEASE)

--- NEW FILE: AvoidUtil.cc ---
#include "AvoidUtil.h"
#include <cmath>
#include <iostream>

const char* AvoidUtilInvalidConstructorArguments::what() const throw()
{
   return "illegal constructor arguments";
}

const char* AvoidUtilIllegalAvoidingThreshold::what() const throw()
{
   return "elliptical proximity safety must have a non zero longitudinal value";
}

const char* AvoidUtilFindSmallestMarginException::what() const throw()
{
   return "lPos > hPos or hPos is out of range";
}

const char* AvoidUtilBeltPositionOutOfRange::what() const throw()
{
   return "Belt position out of range";
}

const char* AvoidUtilMarginException::what() const throw()
{
   return "Margin must be between 0 and 1";
}

AvoidUtil::AvoidUtil(unsigned int maxVelocity, 
                     unsigned int minEllipseLong, 
                     unsigned int ellipseLong, 
                     unsigned int ellipseLat,
                     double zeroVelSecurityMargin,
                     double reverseVelSecurityMargin,
                     int reverseVel,
                     double rotVelSecurityMargin,
                     int rotVel) :
                     m_maxVelocity(maxVelocity), 
                     m_minEllipseLong(minEllipseLong), 
                     m_ellipseLong(ellipseLong), 
                     m_ellipseLat(ellipseLat),
                     m_zeroVelSecurityMargin(zeroVelSecurityMargin),
                     m_reverseVelSecurityMargin(reverseVelSecurityMargin),
                     m_reverseVel(reverseVel),
                     m_rotVelSecurityMargin(rotVelSecurityMargin),
                     m_rotVel(rotVel)
{
   // The velocity must not be o
   // The max veloity has to be greater or equal than the reverse vel
   // The longitudinal value of the ellipse must be greater than 0 and
   // the longitudinal value must be greater than the lateral value
   // The minimal ellipse longitudinal proximity threshold must be smaller than the max speed longitudinal proximity threshold 
   if((m_maxVelocity == 0) or (static_cast<int>(m_reverseVel)>m_maxVelocity) or  (m_ellipseLong == 0) or (m_ellipseLat == 0) or (m_ellipseLat > m_ellipseLong) or (m_minEllipseLong > m_ellipseLong) or (rotVel ==0))
   {
      throw AvoidUtilInvalidConstructorArguments();
   }

   // The margin must be between 0 and 1
   if((m_zeroVelSecurityMargin<0) or (m_zeroVelSecurityMargin>1) or
      (m_reverseVelSecurityMargin<0) or (m_reverseVelSecurityMargin>1) or
      (m_rotVelSecurityMargin<0) or (m_rotVelSecurityMargin>1))
   {
      throw AvoidUtilInvalidConstructorArguments();
   }

   // The margin must respect a specific order
   if(m_zeroVelSecurityMargin<m_reverseVelSecurityMargin)
   {
      throw AvoidUtilInvalidConstructorArguments();
   }

   // Evaluate the slope of the speed to avoiding distance threshold linear relation
   m_slope = static_cast<float>(m_ellipseLong) / m_maxVelocity; 
   std::cout << "m_slope -> " << m_slope << std::endl; 

   // Evaluate the linear approximation constant for the velocity managment
   m_reverseVel = abs(reverseVel);
   m_reverseVel *= -1;   // Force to be negative
   m_reverseSlope = (0-m_reverseVel) / (m_reverseVelSecurityMargin);
   m_reverseB = m_reverseVel;

   // Evaluation the linear approximation constant for the rotation managment
   m_rotVel = abs(m_rotVel);
   m_slopeRot = (0-m_rotVel) / (1-m_rotVelSecurityMargin); 
   m_rotB = -1*m_slopeRot;
   std::cout << "m_slopeRot -> " << m_slopeRot << std::endl;
   std::cout << "m_rotB -> " << m_rotB << std::endl;
}

AvoidUtil::~AvoidUtil()
{
}

unsigned int AvoidUtil::evalLongAvoidingThres(int velocity) const
{
   // Determine the avoiding distance threshold
   double distance = m_slope*std::abs(static_cast<double>(velocity));
   if(distance<m_minEllipseLong)
   {
      distance = m_minEllipseLong;
   }

   // Prevent exceptions on the construction of the safety belt
   if(distance == 0)
   {
      distance = 1;
   }

   return static_cast<unsigned int>(distance);
}

unsigned int AvoidUtil::evalEllipticalThres(unsigned int theta, unsigned int distanceThres, unsigned int ellipseLong, unsigned int ellipseLat) const
{
   // Exception if the current ellipse has a longitudinal dimension equal to 0
   if(distanceThres == 0)
   {
      throw AvoidUtilIllegalAvoidingThreshold();
   }
   else
   {
      // Express theta in radian
      double thetaRad = theta * M_PI / 180;

      // Evaluate the ratio on the reference ellipse dimension
      double dimRatio = static_cast<double>(distanceThres) / ellipseLong;

      // Evaluate the dimension of the actual proximity ellipse
      double a = ellipseLat * dimRatio;
      double b = distanceThres;

      // Partial calculation (polar ellipse formula)
      double ab = a*b;
      double aa = a*a;
      double bb = b*b;
      double coscos = cos(thetaRad) * cos(thetaRad);
      double sinsin = sin(thetaRad) * sin(thetaRad);
      return static_cast<unsigned int>((ab) / sqrt(bb*coscos + aa*sinsin));
   }
}

unsigned int AvoidUtil::evalEllipticalAvoidingThres(unsigned int theta, unsigned int distanceThres) const
{
   return evalEllipticalThres(theta, distanceThres, m_ellipseLong, m_ellipseLat);
}

bool AvoidUtil::createEllipticalAvoidingThresBelt(unsigned int beltSize, unsigned int distanceThres)
{
   if((beltSize == 0) or (beltSize > 360) or (distanceThres == 0))
   {
      // Beltsize must be between 1 and 360 degree and distanceThres <> 0
      return false;
   }
   else{
      // Determine the angular increment for the belt
      unsigned int angularIncrement = 360 / beltSize;

      // Resize the std::Vector
      m_ellipseBelt.resize(beltSize);

      // Fill the belt with the avoiding distance
      unsigned int min=0;
      for(unsigned int i = 0; i<beltSize; ++i)
      {
           m_ellipseBelt[i] = evalEllipticalAvoidingThres(i*angularIncrement, distanceThres);
           // Check if the minimal safety protection is respected
           if(m_minEllipseLong != 0)
           {
              min = evalEllipticalAvoidingThres(i*angularIncrement, m_minEllipseLong);
              if(m_ellipseBelt[i] < min)
              {
                 m_ellipseBelt[i] = min;
              }
           }
      }
      return true;
   }
}

const std::vector<int>& AvoidUtil::getEllipticalAvoidingThresBelt()
{
   return m_ellipseBelt;
}

/*bool AvoidUtil::createEllipticalDirectionChangeThresBelt(unsigned int beltSize)
{
   if((beltSize == 0) or (beltSize > 360))
   {
      // Beltsize must be between 1 and 360 degree and distanceThres <> 0
      return false;
   }
   else{
      // Determine the angular increment for the belt
      unsigned int angularIncrement = 360 / beltSize;

      // Resize the std::Vector
      m_directionChangeBelt.resize(beltSize);

      // Fill the belt with the avoiding distance
      for(unsigned int i = 0; i < beltSize; ++i)
      {
           m_directionChangeBelt[i] = evalEllipticalDirectionChangeThres(i*angularIncrement);
      }
      return true;
   }
}

const std::vector<unsigned int>& AvoidUtil::getEllipticalDirectionChangeThresBelt()
{
   return m_directionChangeBelt;
}*/

bool AvoidUtil::createProximityMarginBelt(std::vector<int> rangeBelt)
{
   if(rangeBelt.size() != m_ellipseBelt.size())
   {
      return false;
   }
   else
   {
      m_proximityMarginBelt.resize(m_ellipseBelt.size());
      for(unsigned int i = 0; i < m_ellipseBelt.size(); ++i)
      {
         m_proximityMarginBelt[i] = (static_cast<double>(rangeBelt[i]) / m_ellipseBelt[i]);
      }
      return true;
   }
}

const std::vector<double>& AvoidUtil::getProximityMarginBelt()
{
   return m_proximityMarginBelt;
}

void AvoidUtil::findSmallestProximityMarginAndPosition(unsigned int lPos, unsigned int hPos, double& margin, unsigned int& pos)
{
   // Validate exception cases
   if((lPos > hPos) || (hPos >= m_proximityMarginBelt.size()))
   {
      throw AvoidUtilFindSmallestMarginException();
   }
   else
   {
      // Find the smallest margin between lPos and hPos and memorize its position
      double smallestMargin=m_proximityMarginBelt[lPos];
      unsigned int position=lPos;
      for(unsigned int i=lPos+1; i<=hPos; ++i){
         if(m_proximityMarginBelt[i]<smallestMargin)
         {
            position = i;
            smallestMargin = m_proximityMarginBelt[i];
         }
      }

      // Save results and exit
      margin = smallestMargin;
      pos = position;
   }
}

// Must be generalized for belt <> 12 segments
int AvoidUtil::evalRotationalDirection(int velocity, unsigned int position)
{
   int sign = 1;
   if(velocity >= 0)
   {
      // If position == 3 (straight forward)
      if(position == 3)
      {
         // Turn on the direction of the biggest side margin
         if(m_proximityMarginBelt[0] > m_proximityMarginBelt[6])
         {
            sign = -1;
         }
         else
         {
            sign = 1;
         }
      }
      else if ((position == 11) || (position <=2))
      {
         // Turning left (1) if position in [11, 0, 1, 2]
         sign = 1;
      }
      else
      {
         // Turning right (-1) if position in [4, 5, 6, 7, 8, 9, 10]
         sign = -1;
      }
   }
   else
   {
      // If position == 9 (straight forward)
      if(position == 9)
      {
         // Turn on the direction of the biggest side margin
         if(m_proximityMarginBelt[0] <= m_proximityMarginBelt[6])
         {
            sign = 1;
         }
         else
         {
            sign = -1;
         }
      }
      else if ((position >= 5) && (position <=8))
      {
         // Turning left (1) if position in [5, 6, 7, 8]
         sign = -1;
      }
      else
      {
         // Turning right (-1) if position in [0, 1, 2, 3, 4, 10, 11]
         sign = 1;
      }
   }
   return sign;
}

bool AvoidUtil::isAvoidanceNeeded(std::vector<int> rangeBelt, int velocity, unsigned int &pos, double &margin)
{
   double minMargin1;
   unsigned int minMarginPos1;
   double minMargin2;
   unsigned int minMarginPos2;

   // Robot is going forward or backward?
   if(velocity == 0)
   {
      // Find the smallest proximity margin and its position
      findSmallestProximityMarginAndPosition(0, 11, minMargin1, minMarginPos1);
      if(minMargin1 < 1)
      {
         margin = minMargin1;
         pos = minMarginPos1;
         return true;
      }
      else
      {
         return false;
      }
   }
   if(velocity > 0) 
   {  
      // Forward

      // Find the smallest proximity margin and its position
      findSmallestProximityMarginAndPosition(0, 7, minMargin1, minMarginPos1);
      if(m_proximityMarginBelt[11] < minMargin1)
      {
         minMargin1 = m_proximityMarginBelt[11];
         minMarginPos1 = 11;
      }

      // If any proximity margin is violated
      if(minMargin1 < 1)
      {
         pos = minMarginPos1;
         margin = minMargin1;
         return true;
      }
      else
      {
         return false;
      }
   }
   else
   {
      // Backward

      // Find the smallest proximity margin and its position
      findSmallestProximityMarginAndPosition(5, 11, minMargin1, minMarginPos1);
      findSmallestProximityMarginAndPosition(0, 1,  minMargin2, minMarginPos2);
      if(minMargin1 > minMargin2)
      {
         minMargin1 = minMargin2;
         minMarginPos1 = minMarginPos2;
      }

      // If any proximity margin is violated
      if(minMargin1 < 1)
      {
         pos = minMarginPos1;
         margin = minMargin1;
         return true;
      }
      else
      {
         return false;
      }
   }

   return false;
}
/*
bool AvoidUtil::isDirectionChangeNeeded(unsigned int position, unsigned int rangeValue)
{
   if(position >= m_directionChangeBelt.size())
   {
      throw AvoidUtilBeltPositionOutOfRange();
   }
   else
   {
      return (rangeValue < m_directionChangeBelt[position]);
   }
}*/

int AvoidUtil::evalAvoidingLinVel(int velocity, double margin)
{
   // Margin [0..1]
   if((margin < 0) or (margin > 1))
   {
      throw AvoidUtilMarginException();
   }

   // Deal according of the violation of the different security margin
   int vel=0;
   if(margin <= m_reverseVelSecurityMargin)
   {
      //Reverse velocity
      vel = static_cast<int>(m_reverseSlope * margin + m_reverseB);
      if(velocity<0)
      {
         vel *= -1;
      }
   }
   else if(margin <= m_zeroVelSecurityMargin)
   {
      // Dead zone
      vel = 0;
   }
   else
   {
      // Evaluate linear approximation variable
      double m = velocity / (1-m_zeroVelSecurityMargin);
      double b = velocity - m * 1;
      vel = static_cast<int>(m*margin+b);
   }
   return vel;
}

int AvoidUtil::evalAvoidingRotVel(int velocity, unsigned int pos, double margin)
{
   // Margin [0..1]
   if((margin < 0) or (margin > 1))
   {
      throw AvoidUtilMarginException();
   }

   // Evaluation de rotational velocity according to the margin
   int rot=0;

   // Determine the magnitude
   if(margin <= m_rotVelSecurityMargin)
   {
      rot = m_rotVel;
   }
   else
   {
      rot = static_cast<int>(m_slopeRot * margin + m_rotB);
   }

   // Determine the sign of the rotation
   int sign = evalRotationalDirection(velocity, pos);
   // Going reverse...change the sign
   if(margin <= m_reverseVelSecurityMargin)
   {
      sign *= -1;
   }
   return sign * rot;
}

--- NEW FILE: EllipticalAvoid.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 _ELLIPTICAL_AVOID_CC_
#define _ELLIPTICAL_AVOID_CC_

#include "Behavior.h"
#include "AvoidUtil.h"
#include <exception>

using namespace std;
using namespace FD;

namespace RobotFlow {

class EllipticalAvoid;

DECLARE_NODE(EllipticalAvoid)
REGISTER_BEHAVIOR(EllipticalAvoid)

/*Node

 * @name EllipticalAvoid
 * @category RobotFlow:Behaviors
 * @description No description available
 *
 * @input_name ACTIVATED
 * @input_type bool
 * @input_description Behavior activation
 *
 * @input_name RANGE_BELT
 * @input_type Vector<unsigned int>
 * @input_description Range mesure around the robot
 *
 * @input_name CURRENT_VELOCITY
 * @input_type int
 * @input_description Current velocity of the robot
 *
 * @output_name VELOCITY
 * @output_type int
 * @output_description Velocity value
 *
 * @output_name ROTATION
 * @output_type int
 * @output_description Rotation value
 *
 * @output_name EXPLOITATION
 * @output_description Behavior exploitation.
 *
 * @parameter_name MAX_VELOCITY
 * @parameter_type int
 * @parameter_value 600
 * @parameter_description Robot maximal forward velocity
 *
 * @parameter_name MIN_ELLIPSE_LONG
 * @parameter_type int
 * @parameter_value 800
 * @parameter_description Longitudinal safey distance regardless of the speed
 *
 * @parameter_name ELLIPSE_LONG
 * @parameter_type int
 * @parameter_value 5000
 * @parameter_description Longitudinal safety distance at MAX_VELOCITY speed
 *
 * @parameter_name ELLIPSE_LAT
 * @parameter_type int
 * @parameter_value 1000
 * @parameter_description Lateral safety distance at MAX_VELOCITY speed
 *
 * @parameter_name ZERO_VEL_SECURITY_MARGIN
 * @parameter_type float
 * @parameter_value 0.5
 * @parameter_description Security margin to apply a zero velocity value [0..1]
 *
 * @parameter_name REVERSE_VEL_SECURITY_MARGIN
 * @parameter_type float
 * @parameter_value 0.2
 * @parameter_description Security margin to reverse the direction of motion [0..1]
 *
 * @parameter_name REVERSE_VEL
 * @parameter_type int
 * @parameter_value 100
 * @parameter_description Reverse velocity to apply when the margin of security is below REVERSE_VEL_SECURITY_MARGIN
 *
 * @parameter_name ROT_VEL_SECURITY_MARGIN
 * @parameter_type float
 * @parameter_value 0.5
 * @parameter_description Security margin to apply the ROT_VEL value
 *
 * @parameter_name ROT_VEL
 * @parameter_type int
 * @parameter_value 20
 * @parameter_description Maximal rotational velocity to apply when avoiding
 *
 END*/

class EllipticalAvoidException: public std::exception
{
  virtual const char* what() const throw()
  {
     return "General exception in behaviors EllipticalAvoid";
  }
};

class EllipticalAvoid : public Behavior {

  //inputs
  int rangeID;
  int velocityInID;

  //outputs
  int velocityOutID;
  int rotationID;

  // Parameters
  unsigned int m_maxVelocity;   
  unsigned int m_minEllipseLong;
  unsigned int m_ellipseLong;
  unsigned int m_ellipseLat;
  double m_zeroVelSecurityMargin;
  double m_reverseVelSecurityMargin;
  int m_reverseVel;
  double m_rotVelSecurityMargin;
  int m_rotVel; 

  // Utility class
  AvoidUtil* avoidUtil;

public:

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

     //inputs
     rangeID = addInput("RANGE_BELT");
     velocityInID = addInput("CURRENT_VELOCITY");

     //outputs
     velocityOutID = addOutput("VELOCITY");
     rotationID = addOutput("ROTATION");

     // Parameters

     m_maxVelocity = dereference_cast<int>(parameters.get("MAX_VELOCITY"));
     m_minEllipseLong = dereference_cast<int>(parameters.get("MIN_ELLIPSE_LONG"));
     m_ellipseLong = dereference_cast<int>(parameters.get("ELLIPSE_LONG"));
     m_ellipseLat = dereference_cast<int>(parameters.get("ELLIPSE_LAT"));
     m_zeroVelSecurityMargin = static_cast<double>(dereference_cast<float>(parameters.get("ZERO_VEL_SECURITY_MARGIN")));
     m_reverseVelSecurityMargin = static_cast<double>(dereference_cast<float>(parameters.get("REVERSE_VEL_SECURITY_MARGIN")));
     m_reverseVel = dereference_cast<int>(parameters.get("REVERSE_VEL"));
     m_rotVelSecurityMargin = static_cast<double>(dereference_cast<float>(parameters.get("ROT_VEL_SECURITY_MARGIN")));
     m_rotVel = dereference_cast<int>(parameters.get("ROT_VEL"));

     // Utility class initialization
     try
     {
        avoidUtil = new AvoidUtil(m_maxVelocity, 
                               m_minEllipseLong, 
                               m_ellipseLong, 
                               m_ellipseLat, 
                               m_zeroVelSecurityMargin,
                               m_reverseVelSecurityMargin,
                               m_reverseVel,
                               m_rotVelSecurityMargin,
                               m_rotVel);
     }
     catch (AvoidUtilInvalidConstructorArguments)
     {
        throw EllipticalAvoidException();
     }
   }

   ~EllipticalAvoid()
   {
      delete avoidUtil;
   }

   void calculate_behavior(int output_id, int count, Buffer &out) {
      //std::cout << "EllipticalAvoid : output_id -> " << output_id << std::endl;
     //std::cout << "EllipticalAvoid -> getInput1" << std::endl;
     ObjectRef rangeValueRef = getInput(rangeID,count);
     //std::cout << "EllipticalAvoid -> getInput2s" << std::endl;
     ObjectRef velInRef = getInput(velocityInID,count);

     //std::cout << "range, vel -> " << rangeValueRef->isNil() << "," << velInRef->isNil() << std::endl;
     if((!rangeValueRef->isNil()) && (!velInRef->isNil())){
        // Cast the range belt
        Vector<int> &rangeValue = object_cast<Vector<int> >(getInput(rangeID,count));

        // Get the current velocity of the robot
        int velIn = dereference_cast<int>(velInRef);

        // According to the actual velocity, create the safety belt
        unsigned int distThres = avoidUtil->evalLongAvoidingThres(velIn);
        if(!avoidUtil->createEllipticalAvoidingThresBelt(rangeValue.size(), distThres))
        {
           throw EllipticalAvoidException();   
        }

        // Compare the current range belt to the safety belt
        if(!avoidUtil->createProximityMarginBelt(rangeValue))
        {
           throw EllipticalAvoidException();   
        }

        // Avoid (if needed)
        unsigned int pos = 1000;
        double margin = -1;
        bool avoid = avoidUtil->isAvoidanceNeeded(rangeValue, velIn, pos, margin);
        std::cout << "avoid -> " << avoid << std::endl;
        std::cout << "Pos -> " << pos << std::endl;
        std::cout << "Margin -> " << margin << std::endl;
        if(!avoid)
        {
           // No avoidance needed
           (*outputs[velocityOutID].buffer)[count] = nilObject;
           (*outputs[rotationID].buffer)[count] = nilObject;
        }
        else
        {
           int velOut = avoidUtil->evalAvoidingLinVel(velIn, margin);
           int rotOut = avoidUtil->evalAvoidingRotVel(velIn, pos, margin);
           (*outputs[velocityOutID].buffer)[count] = ObjectRef(Int::alloc(velOut));
           (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(rotOut));;
        }
     }
     else{
        (*outputs[velocityOutID].buffer)[count] = nilObject;
        (*outputs[rotationID].buffer)[count] = nilObject;
        std::cout << "inputs -> nilObject" << std::endl;
     }
   }//calculate_behavior
};

}//namespace RobotFlow
#endif

Index: AvoidPat.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidPat.cc,v
retrieving revision 1.14
retrieving revision 1.15
diff -C2 -d -r1.14 -r1.15
*** AvoidPat.cc	7 Jul 2005 04:56:45 -0000	1.14
--- AvoidPat.cc	12 Dec 2005 18:27:49 -0000	1.15
***************
*** 44,50 ****
   * @input_description Range mesure around the robot
   *
!  * @input_name ACTUAL_VELOCITY
   * @input_type int
!  * @input_description Actual velocity of the robot
   *
   * @output_name VELOCITY
--- 44,50 ----
   * @input_description Range mesure around the robot
   *
!  * @input_name CURRENT_VELOCITY
   * @input_type int
!  * @input_description Current velocity of the robot
   *
   * @output_name VELOCITY
***************
*** 62,81 ****
   * @parameter_type int
   * @parameter_value 200
!  * @parameter_description Maximum velocity allowed (mm/s).
   *
   * @parameter_name MIN_AVOID_VELOCITY
   * @parameter_type int
!  * @parameter_value 75
!  * @parameter_description Minimum velocity to help the rotation (mm/s).
   *
   * @parameter_name MAX_ROTATION
   * @parameter_type int
   * @parameter_value 15
!  * @parameter_description Maximum rotation allowed (degree/s).
   *
   * @parameter_name MIN_ROTATION
   * @parameter_type int
   * @parameter_value 5
!  * @parameter_description Minimum rotation to get something going (degree/s).
   *
   * @parameter_name MIN_ROTATION_GAIN
--- 62,81 ----
   * @parameter_type int
   * @parameter_value 200
!  * @parameter_description Maximum avoiding velocity allowed (mm/s).
   *
   * @parameter_name MIN_AVOID_VELOCITY
   * @parameter_type int
!  * @parameter_value 0
!  * @parameter_description Minimum avoiding velocity to help the rotation (mm/s).
   *
   * @parameter_name MAX_ROTATION
   * @parameter_type int
   * @parameter_value 15
!  * @parameter_description Maximum avoiding rotational speed allowed (degree/s).
   *
   * @parameter_name MIN_ROTATION
   * @parameter_type int
   * @parameter_value 5
!  * @parameter_description Minimum avoiding rotational speed (to get something going) (degree/s).
   *
   * @parameter_name MIN_ROTATION_GAIN
***************
*** 87,91 ****
   * @parameter_type int
   * @parameter_value 300
!  * @parameter_description Maximum distance of the protection (mm)
   *
   * @parameter_name MAX_VEL_CORRECTION
--- 87,91 ----
   * @parameter_type int
   * @parameter_value 300
!  * @parameter_description Velocity to consider MAX_DISTANCE to start avoiding (mm/s)
   *
   * @parameter_name MAX_VEL_CORRECTION
***************
*** 97,106 ****
   * @parameter_type int
   * @parameter_value 600
!  * @parameter_description Maximum distance of the protection (mm)
   *
   * @parameter_name MIN_DISTANCE
   * @parameter_type int
   * @parameter_value 150
!  * @parameter_description Minimum distance of the protection (mm)
   *
   * @parameter_name MARGIN_TOL
--- 97,106 ----
   * @parameter_type int
   * @parameter_value 600
!  * @parameter_description Avoiding will occur if an obstacle is detected below MAX_DISTANCE from the longitidunal axis of the robot at MAX_ROBOT_VELOCITY (mm)
   *
   * @parameter_name MIN_DISTANCE
   * @parameter_type int
   * @parameter_value 150
!  * @parameter_description Below this distance, avoiding will occur regardless of the current velocity (mm)
   *
   * @parameter_name MARGIN_TOL
***************
*** 127,136 ****
   * @parameter_type int
   * @parameter_value 5
!  * @parameter_description Rotation to apply when going backward (mm/s)
   *
   * @parameter_name BACKWARD_ITERATION
   * @parameter_type int
   * @parameter_value 5
!  * @parameter_description Number of iteration to apply backward velocity (mm/s)
   *
   END*/
--- 127,136 ----
   * @parameter_type int
   * @parameter_value 5
!  * @parameter_description Rotational speed to apply when going backward (mm/s)
   *
   * @parameter_name BACKWARD_ITERATION
   * @parameter_type int
   * @parameter_value 5
!  * @parameter_description Number of iteration to apply the backward outputs
   *
   END*/
***************
*** 166,170 ****
    // Slope of the line used to determine security distance
    float m_dist_slope;
-   float m_dist_b;

    float m_vel_slope;
--- 166,169 ----
***************
*** 256,260 ****
       //inputs
       rangeID = addInput("RANGE_BELT");
!      velocityInID = addInput("ACTUAL_VELOCITY");

       //outputs
--- 255,259 ----
       //inputs
       rangeID = addInput("RANGE_BELT");
!      velocityInID = addInput("CURRENT_VELOCITY");

       //outputs
***************
*** 280,286 ****
       if(m_backward_iter<1) m_backward_iter = 1;

!      m_dist_slope = (1.0*m_max_distance - m_min_distance) / m_max_robot_vel;
!      m_dist_b = m_max_distance - m_dist_slope * m_max_robot_vel; 
     }

     void calculate_behavior(int output_id, int count, Buffer &out) {
--- 279,285 ----
       if(m_backward_iter<1) m_backward_iter = 1;

!      m_dist_slope = (1.0 * m_max_distance) / m_max_robot_vel;
     }
+    

     void calculate_behavior(int output_id, int count, Buffer &out) {
***************
*** 313,317 ****
          }
          else{
!             // Get the actual velocity of the robot
              int velIn = m_max_robot_vel;
              if(!velInRef->isNil()){
--- 312,316 ----
          }
          else{
!             // Get the current velocity of the robot
              int velIn = m_max_robot_vel;
              if(!velInRef->isNil()){
***************
*** 321,329 ****
              }

!             // Determine the security distance
!             float securityDist = 0;
!             if(abs(velIn) < m_min_avoid_vel)
                  securityDist = m_min_distance;
-             else securityDist = m_dist_slope*abs(velIn) + m_dist_b;

              // Evaluate the security margin around the robot
--- 320,327 ----
              }

!             // Determine the avoiding distance threshold
!             float securityDist = m_dist_slope*abs(velIn);
!             if(securityDist<m_min_distance)
                  securityDist = m_min_distance;

              // Evaluate the security margin around the robot
***************
*** 336,343 ****
              int i = 0;
              for(iterRange = rangeValue.begin(); iterRange != rangeValue.end(); ++iterRange){
                  // Apply the side factor on range 2,3,4,8,9,10
                  if(((i>=2) && (i<=4)) || ((i>=8) && (i<=10)))
!                     *iterMargin = 1 + (*iterRange - 1.0*securityDist*m_side_factor) / (securityDist*m_side_factor);
                  else *iterMargin = 1 + (*iterRange - 1.0*securityDist) / securityDist;
                  if(*iterMargin > 1) *iterMargin = 1;
                  ++iterMargin;
--- 334,343 ----
              int i = 0;
              for(iterRange = rangeValue.begin(); iterRange != rangeValue.end(); ++iterRange){
+ 		    
                  // Apply the side factor on range 2,3,4,8,9,10
                  if(((i>=2) && (i<=4)) || ((i>=8) && (i<=10)))
!                      *iterMargin = 1 + (*iterRange - 1.0*securityDist*m_side_factor) / (securityDist*m_side_factor);
                  else *iterMargin = 1 + (*iterRange - 1.0*securityDist) / securityDist;
+                 
                  if(*iterMargin > 1) *iterMargin = 1;
                  ++iterMargin;
***************
*** 345,352 ****
              }

!             // Find the smallest security margin ignoring range 5, 6 and 7
              float min_margin = securityMargin[0];
              int pos_min_margin = 0;
!             for(int i = 1; i<securityMargin.size(); ++i){
                  if(((i<5) || (i>7)) && (securityMargin[i]<min_margin)){
                      min_margin = securityMargin[i];
--- 345,352 ----
              }

!             // Find the smallest security margin ignoring range 5, 6 and 7 (back sensor...going forward)
              float min_margin = securityMargin[0];
              int pos_min_margin = 0;
!             for(int i = 1; i < securityMargin.size(); ++i){
                  if(((i<5) || (i>7)) && (securityMargin[i]<min_margin)){
                      min_margin = securityMargin[i];
***************
*** 444,447 ****
--- 444,449 ----
     }//calculate_behavior

+    
+    
  };

[Robotflow-cvs] RobotFlow/Behaviors/src AvoidUtil.cc,NONE,1.1 EllipticalAvoid.cc,NONE,1.1 AvoidPat.c

[Robotflow-cvs] RobotFlow/Behaviors/src AvoidUtil.cc,NONE,1.1 EllipticalAvoid.cc,NONE,1.1 AvoidPat.cc,1.14,1.15 Makefile.am,1.20,1.21