Menu
▾
▴
[Robotflow-cvs] RobotFlow/Behaviors/src AvoidUtil.cc,1.1,1.2 EllipticalAvoid.cc,1.1,1.2 GotoPat.cc,1.5,1.6
|
From: Fernyqc <fe...@us...> - 2005-12-13 14:47:28
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv31472 Modified Files: AvoidUtil.cc EllipticalAvoid.cc GotoPat.cc Log Message: Addition of the memory on the EllipticalAvoid behavior; Addition of the TYPICAL_VELOCITY_AVOID principle; Index: EllipticalAvoid.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/EllipticalAvoid.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** EllipticalAvoid.cc 12 Dec 2005 18:27:49 -0000 1.1 --- EllipticalAvoid.cc 13 Dec 2005 14:47:17 -0000 1.2 *************** *** 43,47 **** * * @input_name RANGE_BELT ! * @input_type Vector<unsigned int> * @input_description Range mesure around the robot * --- 43,47 ---- * * @input_name RANGE_BELT ! * @input_type Vector<int> * @input_description Range mesure around the robot * *************** *** 61,70 **** * @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 --- 61,70 ---- * @output_description Behavior exploitation. * ! * @parameter_name TYPICAL_AVOIDANCE_VELOCITY * @parameter_type int ! * @parameter_value 300 ! * @parameter_description Typical velocity to apply when avoiding (mm/s) * ! * @parameter_name MIN_ELLIPSE_LONG * @parameter_type int * @parameter_value 800 *************** *** 74,83 **** * @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 --- 74,83 ---- * @parameter_type int * @parameter_value 5000 ! * @parameter_description Longitudinal safety distance at TYPICAL_AVOIDANCE_VELOCITY speed * * @parameter_name ELLIPSE_LAT * @parameter_type int * @parameter_value 1000 ! * @parameter_description Lateral safety distance at TYPICAL_AVOIDANCE_VELOCITY speed * * @parameter_name ZERO_VEL_SECURITY_MARGIN *************** *** 106,109 **** --- 106,114 ---- * @parameter_description Maximal rotational velocity to apply when avoiding * + * @parameter_name NB_AVOIDANCE + * @parameter_type int + * @parameter_value 5 + * @parameter_description Minimal numbers of iteration to apply avoidance + * END*/ *************** *** 127,131 **** // Parameters ! unsigned int m_maxVelocity; unsigned int m_minEllipseLong; unsigned int m_ellipseLong; --- 132,136 ---- // Parameters ! unsigned int m_typVelocity; unsigned int m_minEllipseLong; unsigned int m_ellipseLong; *************** *** 136,139 **** --- 141,145 ---- double m_rotVelSecurityMargin; int m_rotVel; + int m_nbAvoidance; // Utility class *************** *** 154,159 **** // 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")); --- 160,164 ---- // Parameters ! m_typVelocity = dereference_cast<int>(parameters.get("TYPICAL_AVOIDANCE_VELOCITY")); m_minEllipseLong = dereference_cast<int>(parameters.get("MIN_ELLIPSE_LONG")); m_ellipseLong = dereference_cast<int>(parameters.get("ELLIPSE_LONG")); *************** *** 164,172 **** 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, --- 169,178 ---- m_rotVelSecurityMargin = static_cast<double>(dereference_cast<float>(parameters.get("ROT_VEL_SECURITY_MARGIN"))); m_rotVel = dereference_cast<int>(parameters.get("ROT_VEL")); + m_nbAvoidance = dereference_cast<int>(parameters.get("NB_AVOIDANCE")); // Utility class initialization try { ! avoidUtil = new AvoidUtil(m_typVelocity, m_minEllipseLong, m_ellipseLong, *************** *** 196,199 **** --- 202,211 ---- ObjectRef velInRef = getInput(velocityInID,count); + // Memory to prevent oscillation (avoiding / not avoiding) + static int memory = m_nbAvoidance; + static bool avoidActivated = false; + static unsigned int lastPos = 0; + static double lastMargin = -1; + //std::cout << "range, vel -> " << rangeValueRef->isNil() << "," << velInRef->isNil() << std::endl; if((!rangeValueRef->isNil()) && (!velInRef->isNil())){ *************** *** 221,239 **** 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));; } } --- 233,290 ---- double margin = -1; bool avoid = avoidUtil->isAvoidanceNeeded(rangeValue, velIn, pos, margin); ! ! // Memory managment ! if(avoid) { ! if(!avoidActivated) ! { ! avoidActivated = true; ! memory = m_nbAvoidance; ! } ! lastPos = pos; ! lastMargin = margin; } else { + if(avoidActivated && memory != 0) + { + pos = lastPos; + margin = lastMargin; + avoid = true; + } + else + { + avoidActivated = false; + } + } + /* + std::cout << "avoid -> " << avoid << std::endl; + std::cout << "avoidActivated -> " << avoidActivated << std::endl; + std::cout << "memory -> " << memory << std::endl; + std::cout << "velIn -> " << velIn << std::endl; + std::cout << "Pos -> " << pos << std::endl; + std::cout << "Margin -> " << margin << std::endl; + */ + if(avoid) + { + --memory; + if(memory == 0) + { + avoidActivated = false; + } + + // Compute the behavioral response 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)); ! std::cout << "velOut -> " << velOut << std::endl; ! } ! else ! { ! // No avoidance needed ! (*outputs[velocityOutID].buffer)[count] = nilObject; ! (*outputs[rotationID].buffer)[count] = nilObject; ! std::cout << "velOut -> nilObject" << std::endl; } } Index: GotoPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/GotoPat.cc,v retrieving revision 1.5 retrieving revision 1.6 diff -C2 -d -r1.5 -r1.6 *** GotoPat.cc 5 Jul 2005 23:04:29 -0000 1.5 --- GotoPat.cc 13 Dec 2005 14:47:17 -0000 1.6 *************** *** 169,173 **** yPosID = addInput("Y_POS"); - //outputs rotationID = addOutput("ROTATION"); --- 169,172 ---- *************** *** 231,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;*/ ! // Distance to the desired position --- 230,239 ---- 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; // Distance to the desired position Index: AvoidUtil.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidUtil.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** AvoidUtil.cc 12 Dec 2005 18:27:49 -0000 1.1 --- AvoidUtil.cc 13 Dec 2005 14:47:17 -0000 1.2 *************** *** 28,32 **** } ! AvoidUtil::AvoidUtil(unsigned int maxVelocity, unsigned int minEllipseLong, unsigned int ellipseLong, --- 28,32 ---- } ! AvoidUtil::AvoidUtil(unsigned int typVelocity, unsigned int minEllipseLong, unsigned int ellipseLong, *************** *** 37,41 **** double rotVelSecurityMargin, int rotVel) : ! m_maxVelocity(maxVelocity), m_minEllipseLong(minEllipseLong), m_ellipseLong(ellipseLong), --- 37,41 ---- double rotVelSecurityMargin, int rotVel) : ! m_typVelocity(typVelocity), m_minEllipseLong(minEllipseLong), m_ellipseLong(ellipseLong), *************** *** 52,56 **** // 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(); --- 52,56 ---- // 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_typVelocity == 0) or (static_cast<unsigned int>(m_reverseVel)>m_typVelocity) or (m_ellipseLong == 0) or (m_ellipseLat == 0) or (m_ellipseLat > m_ellipseLong) or (m_minEllipseLong > m_ellipseLong) or (rotVel ==0)) { throw AvoidUtilInvalidConstructorArguments(); *************** *** 72,81 **** // 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; --- 72,83 ---- // Evaluate the slope of the speed to avoiding distance threshold linear relation ! m_slope = static_cast<float>(m_ellipseLong) / m_typVelocity; ! ! // Evaluate the linear approximation constant for the ! m_velSlope = (m_typVelocity-0) / (1-m_zeroVelSecurityMargin); ! m_velB = m_typVelocity - m_velSlope * 1; m_reverseVel = abs(reverseVel); ! m_reverseVel *= -1; // Force to be negative m_reverseSlope = (0-m_reverseVel) / (m_reverseVelSecurityMargin); m_reverseB = m_reverseVel; *************** *** 85,90 **** 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; } --- 87,90 ---- *************** *** 160,164 **** // Fill the belt with the avoiding distance ! unsigned int min=0; for(unsigned int i = 0; i<beltSize; ++i) { --- 160,164 ---- // Fill the belt with the avoiding distance ! int min=0; for(unsigned int i = 0; i<beltSize; ++i) { *************** *** 433,439 **** { // Evaluate linear approximation variable ! double m = velocity / (1-m_zeroVelSecurityMargin); ! double b = velocity - m * 1; ! vel = static_cast<int>(m*margin+b); } return vel; --- 433,437 ---- { // Evaluate linear approximation variable ! vel = static_cast<int>(m_velSlope * margin + m_velB); } return vel; |
