Menu
▾
▴
[Robotflow-cvs] RobotFlow/Behaviors/src AvoidPat.cc,1.4,1.5
|
From: Fernyqc <fe...@us...> - 2005-07-04 03:36:35
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv31970 Modified Files: AvoidPat.cc Log Message: - Speed management modification Index: AvoidPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidPat.cc,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** AvoidPat.cc 3 Jul 2005 23:54:19 -0000 1.4 --- AvoidPat.cc 4 Jul 2005 03:36:24 -0000 1.5 *************** *** 99,102 **** --- 99,107 ---- * @parameter_description Minimum distance of the protection (mm) * + * @parameter_name MARGIN_TOL + * @parameter_type float + * @parameter_value 0.5 + * @parameter_description Factor to break linear approximation of the speed calculation [0,1] + * * @parameter_name SIDE_FACTOR * @parameter_type float *************** *** 136,139 **** --- 141,145 ---- int m_min_distance; float m_side_factor; + float m_margin_tol; int m_dist_backward; int m_max_backward_vel; *************** *** 146,149 **** --- 152,185 ---- 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: + case 7: + case 11: + max_speed *= 0.85; + min_speed *= 0.85; + break; + case 2: + case 4: + case 8: + case 10: + max_speed *= 0.65; + min_speed *= 0.65; + break; + case 3: + case 9: + max_speed *= 0.50; + min_speed *= 0.50; + break; + } + + // Evaluation slope and b + slope = (max_speed - min_speed) / (1-margin_tol); + b = min_speed - slope * margin_tol; + return; + } + public: *************** *** 168,171 **** --- 204,208 ---- 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")); *************** *** 175,180 **** m_dist_b = m_max_distance - m_dist_slope * m_max_robot_vel; ! m_vel_slope = (m_max_avoid_vel-m_min_avoid_vel) / (4.0); ! m_vel_b = m_min_avoid_vel; } --- 212,217 ---- 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); } *************** *** 217,337 **** // 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_avoid_vel * min_margin); - //if(velOut < m_min_avoid_vel) velOut = m_min_avoid_vel; ! // 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 ! 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 ! 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; ! 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; ! }*/ ! ! // Rotation version 2 ! switch(ind_min_margin){ ! case 0: rotation = m_max_rot; break; ! case 1: rotation = static_cast<int>(m_max_rot * 0.75); break; ! case 2: rotation = static_cast<int>(m_max_rot * 0.50); break; ! case 3: rotation = static_cast<int>(m_max_rot * 0.25); break; ! case 4: rotation = static_cast<int>(m_max_rot * 0.10); break; ! case 8: rotation = -1*static_cast<int>(m_max_rot * 0.10); break; ! case 9: rotation = -1*static_cast<int>(m_max_rot * 0.25); break; ! case 10: rotation = -1*static_cast<int>(m_max_rot * 0.50); break; ! case 11: rotation = -1*static_cast<int>(m_max_rot * 0.75); break; ! } ! ! // Speed version 2 ! int indice = ind_min_margin; ! if(indice > 4) indice = 12 - indice; ! int velOut = static_cast<int>(m_vel_slope * indice + m_vel_b); // 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 --- 254,304 ---- // 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 *************** *** 342,346 **** 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) --- 309,312 ---- *************** *** 348,355 **** 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{ --- 314,324 ---- 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{ *************** *** 357,362 **** (*outputs[velocityOutID].buffer)[count] = nilObject; (*outputs[rotationID].buffer)[count] = nilObject; ! std::cout << "Avoid Behavior" << std::endl; ! std::cout << "no avoidance" << std::endl; } } --- 326,331 ---- (*outputs[velocityOutID].buffer)[count] = nilObject; (*outputs[rotationID].buffer)[count] = nilObject; ! //std::cout << "Avoid Behavior" << std::endl; ! //std::cout << "no avoidance" << std::endl; } } |
