[Robotflow-cvs] RobotFlow/Behaviors/src AvoidPat.cc,NONE,1.1 Makefile.am,1.17,1.18 RangeFashion.cc,1

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-serv8542

Modified Files:
	Makefile.am RangeFashion.cc SafeVelocityPat.cc 
Added Files:
	AvoidPat.cc 
Log Message:
- First version of AvoidPat;
- Addition of AvoidPat.cc to the Makefile.am;
- The speed management in SafeVelocity has been reviewed;
- Indentation modification in RangeFashion;

Index: Makefile.am
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/Makefile.am,v
retrieving revision 1.17
retrieving revision 1.18
diff -C2 -d -r1.17 -r1.18
*** Makefile.am	2 Jul 2005 02:12:43 -0000	1.17
--- Makefile.am	3 Jul 2005 04:35:47 -0000	1.18
***************
*** 34,38 ****
  	FollowWall.cc \
  	RangeFashion.cc \
! 	SafeVelocityPat.cc

  libBehaviors_la_LDFLAGS = -release $(LT_RELEASE)
--- 34,39 ----
  	FollowWall.cc \
  	RangeFashion.cc \
! 	SafeVelocityPat.cc \
! 	AvoidPat.cc

  libBehaviors_la_LDFLAGS = -release $(LT_RELEASE)

Index: RangeFashion.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/RangeFashion.cc,v
retrieving revision 1.4
retrieving revision 1.5
diff -C2 -d -r1.4 -r1.5
*** RangeFashion.cc	2 Jul 2005 04:05:46 -0000	1.4
--- RangeFashion.cc	3 Jul 2005 04:35:47 -0000	1.5
***************
*** 146,169 ****
  	      int offset = 0;
  	      switch (ind_vectPercept){
! 		case 0: offset = 100; 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 < 10000)){	      
!                   (*vectPercept)[ind_vectPercept] = x_min - offset;
  	      }

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

--- 146,168 ----
  	      int offset = 0;
  	      switch (ind_vectPercept){
!             case 0: offset = 100; 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 < 10000)){	      
!              (*vectPercept)[ind_vectPercept] = x_min - offset;
  	      }

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

Index: SafeVelocityPat.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/SafeVelocityPat.cc,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** SafeVelocityPat.cc	2 Jul 2005 02:12:43 -0000	1.1
--- SafeVelocityPat.cc	3 Jul 2005 04:35:47 -0000	1.2
***************
*** 101,105 ****

     SafeVelocityPat(string nodeName, ParameterSet params)
!    : Behavior(nodeName, params, "SafeVelocity") {

       //inputs
--- 101,105 ----

     SafeVelocityPat(string nodeName, ParameterSet params)
!    : Behavior(nodeName, params, "SafeVelocityPat") {

       //inputs
***************
*** 129,133 ****

           // Determine the security distance
-          cout << "castVel" << std::endl;
           int velIn = dereference_cast<int>(velInRef);
           float securityDist = m*velIn + m_min_distance;
--- 129,132 ----
***************
*** 138,142 ****
           iterMargin = securityMargin.begin();

-          cout << "castRange" << std::endl;
           Vector<int> &rangeValue = object_cast<Vector<int> >(getInput(rangeID,count));
           Vector<int>::iterator iterRange;
--- 137,140 ----
***************
*** 147,193 ****
           }

!          // According to the side og the rotation ponderate the risk of collision
!          cout << "castRotation" << std::endl;
           int rotation = dereference_cast<int>(rotationRef);
!          float riskFront=0, riskBack=0;
           if (rotation < 0){
!              // Risk in the back of the robot (range 6,7,8,9)
!              riskBack = 1 * securityMargin[6] +
!                          3 * securityMargin[7] +
!                          3 * securityMargin[8] +
!                          2 * securityMargin[9];
!              riskBack = riskBack / 9;

!              // Risk in front of the robot (range 0,1,2,3)
!              riskFront = 2 * securityMargin[0] +
!                          3 * securityMargin[1] +
!                          3 * securityMargin[2] +
!                          1 * securityMargin[3];
!              riskFront = riskFront / 9;
           }
           else{
!              // Risk in the back of the robot (range 3,4,5,6)
!              riskBack = 2 * securityMargin[3] +
!                         3 * securityMargin[4] +
!                         3 * securityMargin[5] +
!                         1 * securityMargin[6];
!              riskBack = riskBack / 9;

!              // Risk in front of the robot (range 9,10,11,0)
!              riskFront = 2 * securityMargin[0] +
!                          1 * securityMargin[9] +
!                          3 * securityMargin[10] +
!                          3 * securityMargin[11];
!              riskFront = riskFront / 9;
           }

!          // Adjust the speed according to the risks
!          if (riskBack > 1) riskBack = 1;
!          if (riskFront > 1) riskFront = 1;
!          if(riskFront < riskBack) 
!               velOut = static_cast<int>(velIn * riskFront);
!          else velOut = static_cast<int>(velIn * riskBack);

           // Debugging purpose only
           if(velOut != velIn)
               std::cout << "(velOut != velIn)" << std::endl;
--- 145,193 ----
           }

!          // According to the side of the rotation evaluation de margin of safety
           int rotation = dereference_cast<int>(rotationRef);
!          float marginFront=1, marginBack=1;
           if (rotation < 0){
!              // Margin at the back of the robot (range 6,7,8,9)
!              marginBack = securityMargin[6];
!              for(int i = 7; i<=9; ++i){
!                  if(securityMargin[i] < marginBack){
!                      marginBack = securityMargin[i];
!                  }
!              }

!              // Margin at front of the robot (range 0,1,2,3)
!              marginFront = securityMargin[0];
!              for(int i = 1; i<=3; ++i){
!                  if(securityMargin[i] < marginFront){
!                      marginFront = securityMargin[i];
!                  }
!              }
           }
           else{
!              // Margin at the back of the robot (range 3,4,5,6)
!              marginBack = securityMargin[3];
!              for(int i = 4; i<=6; ++i){
!                  if(securityMargin[i] < marginBack){
!                      marginBack = securityMargin[i];
!                  }
!              }

!              // Margin at front of the robot (range 9,10,11,0)
!              marginFront = securityMargin[0];
!              for(int i = 9; i<=11; ++i){
!                  if(securityMargin[i] < marginFront){
!                      marginFront = securityMargin[i];
!                  }
!              }
           }

!          // Adjust the speed according to the margins
!          if(marginFront < marginBack) 
!               velOut = static_cast<int>(velIn * marginFront);
!          else velOut = static_cast<int>(velIn * marginBack);

           // Debugging purpose only
+          std::cout << "Safe Velocity" << std::endl;
           if(velOut != velIn)
               std::cout << "(velOut != velIn)" << std::endl;
***************
*** 201,209 ****
               std::cout << *iterMargin << " ";
           std::cout << std::endl;
!          std::cout << "riskFront -> " << riskFront << std::endl;
!          std::cout << "riskBack  -> " << riskBack << std::endl;
           std::cout << "velIn     -> " << velIn << std::endl;
           std::cout << "velOut    -> " << velOut << std::endl;
-          std::cout << "rotation  -> " << rotation << std::endl;
       }

--- 201,208 ----
               std::cout << *iterMargin << " ";
           std::cout << std::endl;
!          std::cout << "marginFront -> " << marginFront << std::endl;
!          std::cout << "marginBack  -> " << marginBack << std::endl;
           std::cout << "velIn     -> " << velIn << std::endl;
           std::cout << "velOut    -> " << velOut << std::endl;
       }

--- NEW FILE: AvoidPat.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 _AVOID_PAT_CC_
#define _AVOID_PAT_CC_

#include "Behavior.h"

using namespace std;
using namespace FD;

namespace RobotFlow {

class AvoidPat;

DECLARE_NODE(AvoidPat)
REGISTER_BEHAVIOR(AvoidPat)

/*Node

 * @name AvoidPat
 * @category RobotFlow:Behaviors
 * @description No description available
 *
 * @input_name ACTIVATED
 * @input_type bool
 * @input_description Behavior activation
 *
 * @input_name DIST_OBSTACLE
 * @input_type Vector<int>
 * @input_description Distance of the obstacle to the robot
 *
 * @input_name VELOCITY_IN
 * @input_type int
 * @input_description Actual velocity of the robot
 *
 * @output_name VELOCITY_OUT
 * @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 300
 * @parameter_description Maximum velocity allowed (mm/s).
 *
 * @parameter_name MAX_ROTATION
 * @parameter_type int
 * @parameter_value 25
 * @parameter_description Maximum rotation allowed (degree/s).
 *
 * @parameter_name MIN_ROTATION
 * @parameter_type int
 * @parameter_value 5
 * @parameter_description Maximum rotation allowed (degree/s).
 *
 * @parameter_name MIN_ROTATION_GAIN
 * @parameter_type float
 * @parameter_value 1.5
 * @parameter_description Gain to apply to the MIN_ROTATION value
  *
 * @parameter_name MAX_DISTANCE
 * @parameter_type int
 * @parameter_value 1000
 * @parameter_description Maximum distance of the protection (mm)
 *
 * @parameter_name MIN_DISTANCE
 * @parameter_type int
 * @parameter_value 300
 * @parameter_description Maximum distance of the protection (mm)
 *
 * @parameter_name DIST_BACKWARD
 * @parameter_type int
 * @parameter_value 300
 * @parameter_description Distance to go backward
 *
 END*/

class AvoidPat : public Behavior {

  //inputs
  int rangeID;
  int velocityInID;

  //outputs
  int velocityOutID;
  int rotationID;

  //parameters
  int m_max_vel;
  int m_max_rot;
  int m_min_rot;
  float m_min_rot_gain;
  int m_max_distance;
  int m_min_distance;
  int m_dist_backward;

  // Slope of the line used to determine security distance
  float m_vel_m;

public:

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

     //inputs
     rangeID = addInput("DIST_OBSTACLE");
     velocityInID = addInput("VELOCITY_IN");

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

     // Parameters
     m_max_vel = dereference_cast<int> (parameters.get("MAX_VELOCITY"));
     m_max_rot = dereference_cast<int> (parameters.get("MAX_ROTATION"));
     m_min_rot = dereference_cast<int> (parameters.get("MIN_ROTATION"));
     m_min_rot_gain = dereference_cast<float> (parameters.get("MIN_ROTATION_GAIN"));
     m_max_distance = dereference_cast<int> (parameters.get("MAX_DISTANCE"));
     m_min_distance = dereference_cast<int> (parameters.get("MIN_DISTANCE"));
     m_dist_backward = dereference_cast<int> (parameters.get("DIST_BACKWARD"));

     m_vel_m = (1.0*m_max_distance - m_min_distance) /m_max_vel; 
   }

   void calculate_behavior(int output_id, int count, Buffer &out) {
     ObjectRef rangeValueRef = getInput(rangeID,count);
     ObjectRef velInRef = getInput(velocityInID,count);

     if(!rangeValueRef->isNil()){

        // Get the actual velocity of the robot
        int velIn = m_max_vel;
        /*if(!velInRef->isNil()){
           velIn = dereference_cast<int>(velInRef);
        }*/

        // Determine the security distance
        float securityDist = m_vel_m*velIn + m_min_distance;

        // Evaluate the security margin around the robot
        Vector<float> securityMargin(12);
        Vector<float>::iterator iterMargin;
        iterMargin = securityMargin.begin();

        Vector<int> &rangeValue = object_cast<Vector<int> >(getInput(rangeID,count));
        Vector<int>::iterator iterRange;
        for(iterRange = rangeValue.begin(); iterRange != rangeValue.end(); ++iterRange){
            *iterMargin = 1 + (*iterRange - 1.0*securityDist) / securityDist;
            if(*iterMargin > 1) *iterMargin = 1;
            ++iterMargin;
        }

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

        if(min_margin < 1){
            // Determine linear speed according to the security margin
            int velOut = static_cast<int>(m_max_vel * min_margin);

            // Determine rotational speed
            float rotation;

            // Using different relation depending of the closest obstacle to the robot
            float slope;
            float b;
            switch (ind_min_margin){
                case 2:
                case 3: 
                case 9:
                case 10:
                    // Evaluate the slope and the b at origin for a linear approximation
                    if(min_margin<0.5){
                        slope = (m_max_rot - m_min_rot) / (0.5);
                        b = m_min_rot;
                    }
                    else{
                        slope = (m_max_rot - m_min_rot) / (0.5-1);
                        b = m_max_rot - slope * 0.5;
                    }

                    // Calculate the rotational speed
                    rotation = slope * min_margin + b;

                    // Evaluation de side of the rotation
                    if(ind_min_margin > 3)
                        rotation *= -1;
                    break;

                case 0:
                case 1:
                case 11:
                    /*if(rangeValue[ind_min_margin] < m_dist_backward){
                        // Going backward
                    }
                    else{*/
                        // Going forward
                        // Evaluate the slope and the b at origin for a linear approximation
                        if(min_margin<0.5){
                            slope = (m_max_rot - m_min_rot_gain * m_min_rot) / (0.5);
                            b = m_min_rot_gain * m_min_rot;
                        }
                        else{
                            slope = (m_max_rot - m_min_rot_gain * m_min_rot) / (0.5-1);
                            b = m_max_rot - slope * 0.5;
                        }

                        // Calculate the rotational speed
                        rotation = slope * min_margin + b;

                        // Evaluation de side of the rotation
                        if(ind_min_margin == 11)
                            rotation *= -1;
                        else if(ind_min_margin == 0){
                            // Turn the side of the largest range available
                            if(rangeValue[9] > rangeValue[3])
                                rotation *= -1;
                        }
                        break;
                    /*}
                    break;*/

                case 4:
                case 8:
                    // Determine rotation speed
                    if(min_margin < 0.5) rotation = m_min_rot;
                    else rotation = 0;

                    // Evaluation de side of the rotation
                    if(ind_min_margin==8) rotation *= -1;
                    break;
            }

            // Define FD output
            (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(static_cast<int> (rotation)));
            (*outputs[velocityOutID].buffer)[count] = ObjectRef(Int::alloc(velOut));

            // Debugging purpose only
            std::cout << "Avoid Behavior" << std::endl;
            std::cout << "securityDist -> " << securityDist << std::endl;
            std::cout << "rangeValue -> " << std::endl;
            for(iterRange = rangeValue.begin(); iterRange != rangeValue.end(); ++iterRange)
                std::cout << *iterRange << " ";
            std::cout << std::endl;
            std::cout << "securityMargin.size() -> " << securityMargin.size() << std::endl;
            std::cout << "securityMargin -> " << std::endl;
            for(iterMargin = securityMargin.begin(); iterMargin != securityMargin.end(); ++iterMargin)
                std::cout << *iterMargin << " ";
            std::cout << std::endl;
            std::cout << "min_margin -> " << min_margin << std::endl;
            std::cout << "ind_min_margin -> " << ind_min_margin << std::endl;
            std::cout << "slope -> " << slope << std::endl;
            std::cout << "rotation -> " << rotation << std::endl;
            std::cout << "velocity -> " << velOut << std::endl; 
        }
        else{
            // No avoidance to do
            (*outputs[velocityOutID].buffer)[count] = nilObject;
            (*outputs[rotationID].buffer)[count] = nilObject;
            std::cout << "Avoid Behavior" << std::endl;            
            std::cout << "no avoidance" << std::endl;
        }
     }
     else{
        (*outputs[velocityOutID].buffer)[count] = nilObject;
        (*outputs[rotationID].buffer)[count] = nilObject;
     }
   }//calculate_behavior

};

}//namespace RobotFlow
#endif

[Robotflow-cvs] RobotFlow/Behaviors/src AvoidPat.cc,NONE,1.1 Makefile.am,1.17,1.18 RangeFashion.cc,1

[Robotflow-cvs] RobotFlow/Behaviors/src AvoidPat.cc,NONE,1.1 Makefile.am,1.17,1.18 RangeFashion.cc,1.4,1.5 SafeVelocityPat.cc,1.1,1.2