Menu
▾
▴
[Robotflow-cvs] RobotFlow/Behaviors/src AvoidPat.cc,1.2,1.3 SafeVelocityPat.cc,1.3,1.4
|
From: Fernyqc <fe...@us...> - 2005-07-03 17:39:32
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv17576 Modified Files: AvoidPat.cc SafeVelocityPat.cc Log Message: - Adding abs when calcutating the security distance; - Moving backward in AvoidPat; - Adding parameters side_factor in both behavior; Index: SafeVelocityPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/SafeVelocityPat.cc,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** SafeVelocityPat.cc 3 Jul 2005 07:24:17 -0000 1.3 --- SafeVelocityPat.cc 3 Jul 2005 17:39:21 -0000 1.4 *************** *** 82,85 **** --- 82,90 ---- * @parameter_description Maximum distance of the protection (mm) * + * @parameter_name SIDE_FACTOR + * @parameter_type float + * @parameter_value 0.75 + * @parameter_description Factor to scale de security distance for the side + * END*/ *************** *** 100,103 **** --- 105,109 ---- int m_max_distance; int m_min_distance; + float m_side_factor; // Slope of the line used to determine security distance *************** *** 123,126 **** --- 129,133 ---- m_max_distance = dereference_cast<int> (parameters.get("MAX_DISTANCE")); m_min_distance = dereference_cast<int> (parameters.get("MIN_DISTANCE")); + m_side_factor = dereference_cast<float> (parameters.get("SIDE_FACTOR")); m_slope = (1.0*m_max_distance - m_min_distance) / (m_max_vel - m_min_vel); *************** *** 140,144 **** int velIn = dereference_cast<int>(velInRef); float securityDist = 0; ! if(velIn < m_min_vel) securityDist = m_min_distance; else securityDist = m_slope*velIn + m_b; --- 147,151 ---- int velIn = dereference_cast<int>(velInRef); float securityDist = 0; ! if(abs(velIn) < m_min_vel) securityDist = m_min_distance; else securityDist = m_slope*velIn + m_b; *************** *** 151,158 **** 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; } --- 158,169 ---- 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){ ! 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; } Index: AvoidPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidPat.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** AvoidPat.cc 3 Jul 2005 07:24:17 -0000 1.2 --- AvoidPat.cc 3 Jul 2005 17:39:21 -0000 1.3 *************** *** 99,102 **** --- 99,107 ---- * @parameter_description Minimum distance of the protection (mm) * + * @parameter_name SIDE_FACTOR + * @parameter_type float + * @parameter_value 0.75 + * @parameter_description Factor to scale de security distance for the side + * * @parameter_name DIST_BACKWARD * @parameter_type int *************** *** 104,110 **** * @parameter_description Distance to go backward * END*/ - class AvoidPat : public Behavior { --- 109,119 ---- * @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*/ class AvoidPat : public Behavior { *************** *** 126,130 **** --- 135,141 ---- int m_max_distance; int m_min_distance; + float m_side_factor; int m_dist_backward; + int m_max_backward_vel; // Slope of the line used to determine security distance *************** *** 154,158 **** --- 165,171 ---- m_max_distance = dereference_cast<int> (parameters.get("MAX_DISTANCE")); m_min_distance = dereference_cast<int> (parameters.get("MIN_DISTANCE")); + 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_slope = (1.0*m_max_distance - m_min_distance) / m_max_robot_vel; *************** *** 174,178 **** // Determine the security distance float securityDist = 0; ! if(velIn < m_min_avoid_vel) securityDist = m_min_distance; else securityDist = m_slope*velIn + m_b; --- 187,191 ---- // Determine the security distance float securityDist = 0; ! if(abs(velIn) < m_min_avoid_vel) securityDist = m_min_distance; else securityDist = m_slope*velIn + m_b; *************** *** 185,192 **** 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; } --- 198,210 ---- 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; } *************** *** 238,245 **** 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 --- 256,274 ---- case 1: case 11: ! if(rangeValue[ind_min_margin] < m_dist_backward){ // Going backward + if (velOut > m_max_backward_vel) velOut = m_max_backward_vel; + velOut *= -1; + rotation = m_max_rot; + // Evaluation de side of the rotation + if(ind_min_margin == 1) + rotation *= -1; + else if(ind_min_margin == 0){ + // Turn the side of the largest range available + if(rangeValue[3] > rangeValue[9]) + rotation *= -1; + } } ! else{ // Going forward // Evaluate the slope and the b at origin for a linear approximation *************** *** 264,270 **** rotation *= -1; } ! break; ! /*} ! break;*/ case 4: --- 293,298 ---- rotation *= -1; } ! } ! break; case 4: |
