[Robotflow-cvs] RobotFlow/Behaviors/src AvoidPat.cc,1.7,1.8

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

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

Modified Files:
	AvoidPat.cc 
Log Message:
- Allowing backward motion 


Index: AvoidPat.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidPat.cc,v
retrieving revision 1.7
retrieving revision 1.8
diff -C2 -d -r1.7 -r1.8
*** AvoidPat.cc	4 Jul 2005 11:23:24 -0000	1.7
--- AvoidPat.cc	4 Jul 2005 22:51:45 -0000	1.8
***************
*** 109,121 ****
   * @parameter_description Factor to scale de security distance for the side
   *
!  * @parameter_name DIST_BACKWARD
   * @parameter_type int
   * @parameter_value 200
!  * @parameter_description Distance to go backward
   *
!  * @parameter_name MAX_BACKWARD_VELOCITY
   * @parameter_type int
   * @parameter_value 100
!  * @parameter_description Maximum speed backward (mm/s)
   *
   END*/
--- 109,131 ----
   * @parameter_description Factor to scale de security distance for the side
   *
!  * @parameter_name BACKWARD_DIST
   * @parameter_type int
   * @parameter_value 200
!  * @parameter_description Distance threshold to go backward
   *
!  * @parameter_name BACKWARD_VELOCITY
   * @parameter_type int
   * @parameter_value 100
!  * @parameter_description Backward velocity to apply (mm/s)
!  *
!  * @parameter_name BACKWARD_ROTATION
!  * @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*/
***************
*** 142,147 ****
    float m_side_factor;
    float m_margin_tol;
!   int m_dist_backward;
!   int m_max_backward_vel;
    
    // Slope of the line used to determine security distance
--- 152,160 ----
    float m_side_factor;
    float m_margin_tol;
!   
!   int m_backward_dist;
!   int m_backward_vel;
!   int m_backward_rot;
!   int m_backward_iter;
    
    // Slope of the line used to determine security distance
***************
*** 152,158 ****
    float m_vel_b;
  
!   void computeSpeedApproxParameters(int pos_min_margin, float &max_speed, float &min_speed, float margin_tol, float &slope, float &b){
!       // Decrease maximum and minimum speed according to the position of the minimum margin of security
!       switch(pos_min_margin){
            case 1:
            case 5: 
--- 165,215 ----
    float m_vel_b;
  
!   int computeVel(int pos_min_margin, float min_margin){
!     
!     float velocity=0;
!     float max_speed=m_max_avoid_vel;
!     float min_speed=m_min_avoid_vel;
!     
!     // Decrease maximum and minimum speed according to the position of the minimum margin of security
!     switch(pos_min_margin){
!           case 1:
!           case 5: 
!           case 7:
!           case 11:
!               max_speed *= 1;
!               min_speed *= 1;
!               break;
!           case 2:
!           case 4: 
!           case 8:
!           case 10:
!               max_speed *= 0.80;
!               min_speed *= 0.80;
!               break;
!           case 3:
!           case 9: 
!               max_speed *= 0.75;
!               min_speed *= 0.75;
!               break;
!     }
!       
!     // Evaluation slope and b
!     float slope = (max_speed - min_speed) / (1-m_margin_tol);
!     float b = min_speed - slope * m_margin_tol;
!       
!     // Approximation and offset
!     velocity = slope * min_margin + b;
!     if(velocity < m_min_avoid_vel) velocity = m_min_avoid_vel;
!     return static_cast<int> (velocity);
!   }
!   
!   int computeRot(int pos_min_margin, float min_margin){
!     
!     float rotation=0;
!     float max_speed=m_max_rot;
!     float min_speed=m_min_rot;
!       
!     // Decrease maximum and minimum speed according to the position of the minimum margin of security
!     switch(pos_min_margin){
            case 1:
            case 5: 
***************
*** 174,183 ****
                min_speed *= 0.60;
                break;
!       }
        
!       // Evaluation slope and b
!       slope = (max_speed - min_speed) / (1-margin_tol);
!       b = min_speed - slope * margin_tol;
!       return;
    }
    
--- 231,244 ----
                min_speed *= 0.60;
                break;
!     }
        
!     // Evaluation slope and b
!     float slope = (max_speed - min_speed) / (1-m_margin_tol);
!     float b = min_speed - slope * m_margin_tol;
!       
!     // Approximation and offset
!     rotation = slope * min_margin + b;
!     if(rotation < m_min_rot) rotation = m_min_rot;
!     return static_cast<int> (rotation);
    }
    
***************
*** 196,217 ****
  
       // Parameters
!      m_max_avoid_vel = dereference_cast<int> (parameters.get("MAX_AVOID_VELOCITY"));
!      m_min_avoid_vel = dereference_cast<int> (parameters.get("MIN_AVOID_VELOCITY"));
!      m_max_robot_vel = dereference_cast<int> (parameters.get("MAX_ROBOT_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_margin_tol = dereference_cast<float> (parameters.get("MARGIN_TOL"));
!      m_side_factor = dereference_cast<float> (parameters.get("SIDE_FACTOR"));
!      m_dist_backward = dereference_cast<int> (parameters.get("DIST_BACKWARD"));
!      m_max_backward_vel = dereference_cast<int> (parameters.get("MAX_BACKWARD_VELOCITY"));
       
       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; 
- 
- //     m_vel_slope = (m_max_avoid_vel-m_min_avoid_vel) / (1.0-m_margin_factor); 
- //     m_vel_b = m_min_avoid_vel - m_vel_slope * (1.0-m_margin_factor);	     
     }
  
--- 257,278 ----
  
       // Parameters
!      m_max_avoid_vel = dereference_cast<int>   (parameters.get("MAX_AVOID_VELOCITY"));
!      m_min_avoid_vel = dereference_cast<int>   (parameters.get("MIN_AVOID_VELOCITY"));
!      m_max_robot_vel = dereference_cast<int>   (parameters.get("MAX_ROBOT_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_margin_tol =    dereference_cast<float> (parameters.get("MARGIN_TOL"));
!      m_side_factor =   dereference_cast<float> (parameters.get("SIDE_FACTOR"));
!      m_backward_dist = dereference_cast<int>   (parameters.get("BACKWARD_DIST"));
!      m_backward_vel =  dereference_cast<int>   (parameters.get("BACKWARD_VELOCITY"));
!      m_backward_rot =  dereference_cast<int>   (parameters.get("BACKWARD_ROTATION"));
!      m_backward_iter = dereference_cast<int>   (parameters.get("BACKWARD_ITERATION"));
!      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; 
     }
  
***************
*** 220,331 ****
       ObjectRef velInRef = getInput(velocityInID,count);
       
       if(!rangeValueRef->isNil()){
           
!         // Get the actual velocity of the robot
!         int velIn = m_max_robot_vel;
!         if(!velInRef->isNil()){
!            velIn = dereference_cast<int>(velInRef);
!         }
!         
!         // 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
!         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;
!         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;
!             ++i;
          }
!         
!         // 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];
!                 pos_min_margin = i;
              }
-         }
- 
-         // If avoidance is needed
-         if(min_margin < 1){
-             
-             // Compute velocity
-             float velocity;
              
!             // Symetrie of the range value
!             int position = pos_min_margin;
!             if(position > 4) position = 12 - position;
              
!             // Get the params of the approximation
!             float slope;
!             float b;
!             float max_vel = 1.0*m_max_avoid_vel;
!             float min_vel = 1.0*m_min_avoid_vel;
!             computeSpeedApproxParameters(position, max_vel, min_vel, m_margin_tol, slope, b);
!                 
!             // Computation
!             velocity = slope * min_margin + b;
!             if(velocity < min_vel) velocity = min_vel;
              
!             // Compute rotation
!             float rotation;
              
!             // Get the params of the approximation
!             float max_rot = 1.0*m_max_rot;
!             float min_rot = 1.0*m_min_rot;
!             computeSpeedApproxParameters(position, max_rot, min_rot, m_margin_tol, slope, b);
                  
!             // Computation
!             rotation = slope * min_margin + b;
!             if(rotation < min_rot) rotation = min_rot;
!             
!             // Side of the rotation
!             if(pos_min_margin>=9) rotation *= -1; 
! 	        
!             // Define FD output
!             (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(static_cast<int> (rotation)));
!             (*outputs[velocityOutID].buffer)[count] = ObjectRef(Int::alloc(static_cast<int> (velocity)));
!             
!             // 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 -> " << 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 << "pos_min_margin -> " << pos_min_margin << std::endl;
!             std::cout << "max_vel -> " << max_vel << std::endl;
!             std::cout << "min_vel -> " << min_vel << std::endl;
!             std::cout << "max_rot -> " << max_rot << std::endl;
!             std::cout << "min_rot -> " << min_rot << std::endl;
!             std::cout << "rotation -> " << rotation << std::endl;
!             std::cout << "velocity -> " << velocity << 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;
          }
       }
--- 281,417 ----
       ObjectRef velInRef = getInput(velocityInID,count);
       
+      // Static variabe to manage backward movement
+      static bool backward = false;
+      static int backward_iter = 0;
+      static int backward_vel;
+      static int backward_rot;
+      
       if(!rangeValueRef->isNil()){
           
!         if(backward){
!             // FD outputs
!             (*outputs[velocityOutID].buffer)[count] = ObjectRef(Int::alloc(backward_vel));
!             (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(backward_rot));
!             --backward_iter;
!             backward = !(backward_iter == 0);
!             
!             // Debugging purpose
!             std::cout << "Avoid Behavior" << std::endl;
!             std::cout << "Backward -> true" << std::endl;
!             std::cout << "backward_iter -> " << backward_iter << std::endl;
          }
!         else{
!             // Get the actual velocity of the robot
!             int velIn = m_max_robot_vel;
!             if(!velInRef->isNil()){
!                velIn = dereference_cast<int>(velInRef);
              }
              
!             // 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
!             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;
!             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;
!                 ++i;
!             }
              
!             // 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];
!                     pos_min_margin = i;
!                 }
!             }
              
!             // If avoidance is needed
!             if(min_margin < 1){
!                     
!                 // Turning left or right?
!                 int sign = 1;
!                 if(pos_min_margin == 0){
!                     if(rangeValue[3] > rangeValue[9]) sign = -1;
!                     else sign = 1;
!                 }
!                 else if(pos_min_margin <=4)
!                      sign = 1;
!                 else sign = -1;
                  
!                 // Compute velocity and rotation
!                 int velocity;
!                 int rotation;
!                 
!                 // Moving backward ?
!                 switch(pos_min_margin){
!                     case 0: 
!                     case 1:
!                     case 11:
!                         backward = (rangeValue[pos_min_margin] < m_backward_dist);
!                         velocity = m_backward_vel;
!                         rotation = -1*m_backward_rot;
!                         backward_iter = m_backward_iter-1;
!                         backward_vel = velocity;
!                         backward_rot = rotation * sign;
!                         break;
!                 }
!                 
!                 // Moving forward
!                 if(!backward){
! 
!                     // Symetrie of the range value
!                     int position = pos_min_margin;
!                     if(position > 4) position = 12 - position;
!                     
!                     // Compute velocity and rotation
!                     velocity = computeVel(position, min_margin);
!                     rotation = computeRot(position, min_margin);
!                 }
!                 
!                 // Define FD output
!                 velocity *= sign;
!                 rotation *= sign;
!                 (*outputs[velocityOutID].buffer)[count] = ObjectRef(Int::alloc(velocity));
!                 (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(rotation));
!                 
!                 // 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 -> " << 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 << "pos_min_margin -> " << pos_min_margin << std::endl;
!                 std::cout << "rotation -> " << rotation << std::endl;
!                 std::cout << "velocity -> " << velocity << 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;
!             }
          }
       }