[Robotflow-cvs] RobotFlow/Behaviors/src SafeVelocityPat.cc,NONE,1.1 GotoPat.cc,1.3,1.4 Makefile.am,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-serv22921

Modified Files:
	GotoPat.cc Makefile.am RangeFashion.cc 
Added Files:
	SafeVelocityPat.cc 
Log Message:
- Comment the sdt::cout in GotoPat
- Add SafeVelocityPat.cc in Makefile.am
- Comment std::cout in RangeFashion
- First version of SafeVelocityPat

Index: Makefile.am
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/Makefile.am,v
retrieving revision 1.16
retrieving revision 1.17
diff -C2 -d -r1.16 -r1.17
*** Makefile.am	30 Jun 2005 21:15:28 -0000	1.16
--- Makefile.am	2 Jul 2005 02:12:43 -0000	1.17
***************
*** 33,37 ****
  	GotoPat.cc \
  	FollowWall.cc \
!     RangeFashion.cc

  libBehaviors_la_LDFLAGS = -release $(LT_RELEASE)
--- 33,38 ----
  	GotoPat.cc \
  	FollowWall.cc \
! 	RangeFashion.cc \
! 	SafeVelocityPat.cc

  libBehaviors_la_LDFLAGS = -release $(LT_RELEASE)

Index: RangeFashion.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/RangeFashion.cc,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** RangeFashion.cc	1 Jul 2005 01:52:34 -0000	1.2
--- RangeFashion.cc	2 Jul 2005 02:12:43 -0000	1.3
***************
*** 101,110 ****
        }

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

        // Priority #2
--- 101,110 ----
        }

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

        // Priority #2
***************
*** 155,158 ****
--- 155,164 ----
        }

+       /*std::cout << "Laser -> ";
+       for(iterA = vectPercept->begin(); iterA != vectPercept->end(); ++iterA){
+           std::cout << *iterA << " ";
+       }
+       std::cout << std::endl;*/
+       
        // Priority #3
        // Sonars
***************
*** 170,185 ****
        }

!         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)  // Max value IR
              *iterPercept = 10000;
!       }*/
    }

--- 176,198 ----
        }

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

--- NEW FILE: SafeVelocityPat.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 _SAFEVELOCITY_PAT_CC_
#define _SAFEVELOCITY_PAT_CC_

#include "Behavior.h"
#include "Exploitation.h"

using namespace std;
using namespace FD;

namespace RobotFlow {

class SafeVelocityPat;

DECLARE_NODE(SafeVelocityPat)
REGISTER_BEHAVIOR(SafeVelocityPat)

/*Node

 * @name SafeVelocityPat
 * @category RobotFlow:Behaviors
 * @description Safe Velocity behavior
 *
 * @input_name ACTIVATED
 * @input_type bool
 * @input_description Behavior activation.
 *
 * @input_name VELOCITY_IN
 * @input_type int
 * @input_description Desired velocity of the robot
 *
 * @input_name ROTATION
 * @input_type int
 * @input_description Desired rotation of the robot
 *
 * @input_name DIST_OBSTACLE
 * @input_type Vector<int>
 * @input_description Distance of the obstacle to the robot
 *
 * @output_name VELOCITY_OUT
 * @output_type int
 * @output_description Output velocity
 *
 * @output_name EXPLOITATION
 * @output_description Behavior exploitation.
 *
 * @parameter_name MAX_VELOCITY
 * @parameter_type int
 * @parameter_value 500
 * @parameter_description Maximum velocity allowed (mm/s).
 *
 * @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 100
 * @parameter_description Maximum distance of the protection (mm)
 *
 END*/

class SafeVelocityPat : public Behavior {

  //inputs
  int velocityInID;
  int rotationID;
  int rangeID;

  //outputs
  int velocityOutID;

  //parameters
  int m_max_vel;
  int m_max_distance;
  int m_min_distance;

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

public:

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

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

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

     // Parameters
     m_max_vel = dereference_cast<int> (parameters.get("MAX_VELOCITY"));
     m_max_distance = dereference_cast<int> (parameters.get("MAX_DISTANCE"));
     m_min_distance = dereference_cast<int> (parameters.get("MIN_DISTANCE"));

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

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

     int velOut=0;

     if(!rangeValueRef->isNil() && !velInRef->isNil() && !rotationRef->isNil()){

         // Determine the security distance
         cout << "castVel" << std::endl;
         int velIn = dereference_cast<int>(velInRef);
         float securityDist = m*velIn + m_min_distance;

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

         cout << "castRange" << std::endl;
         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;
         }

         // 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;
         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 -> " << std::endl;
         for(iterMargin = securityMargin.begin(); iterMargin != securityMargin.end(); ++iterMargin)
             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;
     }

     (*outputs[velocityOutID].buffer)[count] = ObjectRef(Int::alloc(velOut));
   }//calculate_behavior

};

}//namespace RobotFlow
#endif

Index: GotoPat.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/GotoPat.cc,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** GotoPat.cc	18 Jun 2005 19:42:34 -0000	1.3
--- GotoPat.cc	2 Jul 2005 02:12:43 -0000	1.4
***************
*** 232,241 ****
         double heading = (double)(dereference_cast<int>(headingValue));

!        std::cout << "intermediate: " << intermediate << std::endl;       
         std::cout << "x: " << x << std::endl;
         std::cout << "y: " << y << std::endl;
         std::cout << "xpos: " << xpos << std::endl;
         std::cout << "ypos: " << ypos << std::endl;
!        std::cout << "heading: " << heading << std::endl;

--- 232,241 ----
         double heading = (double)(dereference_cast<int>(headingValue));

!        /*std::cout << "intermediate: " << intermediate << std::endl;       
         std::cout << "x: " << x << std::endl;
         std::cout << "y: " << y << std::endl;
         std::cout << "xpos: " << xpos << std::endl;
         std::cout << "ypos: " << ypos << std::endl;
!        std::cout << "heading: " << heading << std::endl;*/

***************
*** 255,260 ****
            double diffHeading = newHeading*57.2958 - heading;

!           std::cout << "newHeading: " << newHeading*57.2958 << std::endl;
!           std::cout << "diffHeading: " << diffHeading << std::endl;

            // Prevent over 180 degrees correction
--- 255,260 ----
            double diffHeading = newHeading*57.2958 - heading;

!           /*std::cout << "newHeading: " << newHeading*57.2958 << std::endl;
!           std::cout << "diffHeading: " << diffHeading << std::endl;*/

            // Prevent over 180 degrees correction
***************
*** 286,293 ****
            (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(0));
            (*outputs[completedID].buffer)[count] = ObjectRef(Bool::alloc(true));
!           std::cout << "onGoal" << std::endl;
            std::cout << "rotation: " << (*outputs[rotationID].buffer)[count] << std::endl;
            std::cout << "velocity: " << (*outputs[velocityID].buffer)[count] << std::endl;
!           std::cout << std::endl;
         }

--- 286,293 ----
            (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(0));
            (*outputs[completedID].buffer)[count] = ObjectRef(Bool::alloc(true));
!           /*std::cout << "onGoal" << std::endl;
            std::cout << "rotation: " << (*outputs[rotationID].buffer)[count] << std::endl;
            std::cout << "velocity: " << (*outputs[velocityID].buffer)[count] << std::endl;
!           std::cout << std::endl;*/
         }

***************
*** 298,302 ****
          (*outputs[velocityID].buffer)[count] = nilObject;
          (*outputs[completedID].buffer)[count] = nilObject;
!         std::cout << "COMPLETED = NILOBJECT" << std::endl;
       }

--- 298,302 ----
          (*outputs[velocityID].buffer)[count] = nilObject;
          (*outputs[completedID].buffer)[count] = nilObject;
!         //std::cout << "COMPLETED = NILOBJECT" << std::endl;
       }

[Robotflow-cvs] RobotFlow/Behaviors/src SafeVelocityPat.cc,NONE,1.1 GotoPat.cc,1.3,1.4 Makefile.am,1

[Robotflow-cvs] RobotFlow/Behaviors/src SafeVelocityPat.cc,NONE,1.1 GotoPat.cc,1.3,1.4 Makefile.am,1.16,1.17 RangeFashion.cc,1.2,1.3