Menu
▾
▴
robotflow-cvs — CVS changes are sent to this mailing list.
You can subscribe to this list here.
| 2004 |
Jan
|
Feb
|
Mar
|
Apr
|
May
(59) |
Jun
(40) |
Jul
(59) |
Aug
(81) |
Sep
(14) |
Oct
(9) |
Nov
(22) |
Dec
(1) |
|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 2005 |
Jan
(25) |
Feb
(3) |
Mar
(27) |
Apr
(14) |
May
(15) |
Jun
(112) |
Jul
(44) |
Aug
(7) |
Sep
(18) |
Oct
(34) |
Nov
(17) |
Dec
(20) |
| 2006 |
Jan
(12) |
Feb
|
Mar
(1) |
Apr
|
May
|
Jun
(3) |
Jul
(1) |
Aug
|
Sep
|
Oct
|
Nov
(1) |
Dec
(11) |
|
From: Fernyqc <fe...@us...> - 2005-07-06 01:13:06
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv31640 Modified Files: AvoidPat.cc FollowWall.cc Log Message: - Remove logging n the followwall - Minimum range computation in AvoidPat Index: FollowWall.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/FollowWall.cc,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** FollowWall.cc 18 Jun 2005 15:08:20 -0000 1.4 --- FollowWall.cc 6 Jul 2005 01:12:57 -0000 1.5 *************** *** 38,45 **** * @description Follow a wall if present in the robot environment * - * @input_name PARAMS - * @input_type int - * @input_description Nb of laser samples to consider in the computation. Maximum distance to be considered as a wall - * * @input_name ACTIVATED * @input_type bool --- 38,41 ---- *************** *** 50,57 **** * @input_description All lasers reading. * - * @input_name LOG - * @input_type Vector<int> - * @input_description Log the computation - * * @output_name VELOCITY * @output_type int --- 46,49 ---- *************** *** 167,173 **** //inputs - int paramsID; int laserID; - int logID; //outputs --- 159,163 ---- *************** *** 222,226 **** // Evaluate the std.deviation of a vector ! float computeVariance(vector<float>data) { float sumSquare=0; --- 212,216 ---- // Evaluate the std.deviation of a vector ! float computeStdDev(vector<float>data) { float sumSquare=0; *************** *** 236,240 **** // Compute variance ! return (sumSquare-(sumXi*sumXi/n))/(n-1); } --- 226,230 ---- // Compute variance ! return sqrt((sumSquare-(sumXi*sumXi/n))/(n-1)); } *************** *** 242,246 **** // Compute mean and std.deviation float meanBeta = computeMeanValue(angleBeta); ! float varianceBeta = computeVariance(angleBeta) * RAD_DEGREE; // Compute angular correction and the distance to the wall --- 232,236 ---- // Compute mean and std.deviation float meanBeta = computeMeanValue(angleBeta); ! float varianceBeta = computeStdDev(angleBeta) * RAD_DEGREE; // Compute angular correction and the distance to the wall *************** *** 339,343 **** // Logging ! static std::ofstream laserL("laserLeft.txt"); static std::ofstream cmdL("cmdLeft.txt"); static std::ofstream laserR("laserRight.txt"); --- 329,333 ---- // Logging ! /*static std::ofstream laserL("laserLeft.txt"); static std::ofstream cmdL("cmdLeft.txt"); static std::ofstream laserR("laserRight.txt"); *************** *** 357,367 **** // Compute mean and std.deviation float meanBeta = computeMeanValue(angleBetaLeft)*RAD_DEGREE; ! float varianceBeta = computeVariance(angleBetaLeft); cmdL << "log" << logNumber << "\t" << meanBeta << "\t" << varianceBeta << "\t" << aLeft << "\t" << distWallLeft << "\t" << angularCorrectionLeft << "\t" << wallLeft << std::endl; meanBeta = computeMeanValue(angleBetaRight)*RAD_DEGREE; ! varianceBeta = computeVariance(angleBetaRight); cmdR << "log" << logNumber << "\t" << meanBeta << "\t" << varianceBeta << "\t" << aRight << "\t" << distWallRight << "\t" << angularCorrectionLeft << "\t" << wallRight << std::endl; ++logNumber; ! } } --- 347,357 ---- // Compute mean and std.deviation float meanBeta = computeMeanValue(angleBetaLeft)*RAD_DEGREE; ! float varianceBeta = computeStdDev(angleBetaLeft); cmdL << "log" << logNumber << "\t" << meanBeta << "\t" << varianceBeta << "\t" << aLeft << "\t" << distWallLeft << "\t" << angularCorrectionLeft << "\t" << wallLeft << std::endl; meanBeta = computeMeanValue(angleBetaRight)*RAD_DEGREE; ! varianceBeta = computeStdDev(angleBetaRight); cmdR << "log" << logNumber << "\t" << meanBeta << "\t" << varianceBeta << "\t" << aRight << "\t" << distWallRight << "\t" << angularCorrectionLeft << "\t" << wallRight << std::endl; ++logNumber; ! }*/ } *************** *** 408,414 **** //Inputs - paramsID = addInput("PARAMS"); laserID = addInput("LASERS"); - logID = addInput("LOG"); //Outputs --- 398,402 ---- *************** *** 518,524 **** void calculate_behavior(int output_id, int count, Buffer &out) { - // Get behavior params - ObjectRef paramsValue = getInput(paramsID,count); - // Get laser readings Vector<int> &laserReading = object_cast<Vector<int> >(getInput(laserID,count)); --- 506,509 ---- *************** *** 526,530 **** // Toggle logic to log the sample bool toLog = false; ! static bool log = false; Vector<int> &logValue = object_cast<Vector<int> >(getInput(logID,count)); if (logValue[0] != log) --- 511,515 ---- // Toggle logic to log the sample bool toLog = false; ! /*static bool log = false; Vector<int> &logValue = object_cast<Vector<int> >(getInput(logID,count)); if (logValue[0] != log) *************** *** 532,536 **** toLog = true; log = logValue[0]; ! } //if (!paramsValue->isNil() && laserReading.size() != 0) --- 517,521 ---- toLog = true; log = logValue[0]; ! }*/ //if (!paramsValue->isNil() && laserReading.size() != 0) *************** *** 604,608 **** // Motor cmd debug ! static std::ofstream cmdMotor("motorCmd.txt"); static int logNb = 0; static int header=1; --- 589,593 ---- // Motor cmd debug ! /*static std::ofstream cmdMotor("motorCmd.txt"); static int logNb = 0; static int header=1; *************** *** 614,618 **** cmdMotor << "log" << logNb << "\t" << angularCorrection << "\t" << speedInd << "\t" << velocity << "\t" << rotation << std::endl; ++logNb; ! } // Send motor command --- 599,603 ---- cmdMotor << "log" << logNb << "\t" << angularCorrection << "\t" << speedInd << "\t" << velocity << "\t" << rotation << std::endl; ++logNb; ! }*/ // Send motor command Index: AvoidPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidPat.cc,v retrieving revision 1.8 retrieving revision 1.9 diff -C2 -d -r1.8 -r1.9 *** AvoidPat.cc 4 Jul 2005 22:51:45 -0000 1.8 --- AvoidPat.cc 6 Jul 2005 01:12:57 -0000 1.9 *************** *** 300,303 **** --- 300,305 ---- std::cout << "Backward -> true" << std::endl; std::cout << "backward_iter -> " << backward_iter << std::endl; + std::cout << "backward_vel -> " << backward_vel << std::endl; + std::cout << "backward_rot -> " << backward_rot << std::endl; } else{ |
|
From: Fernyqc <fe...@us...> - 2005-07-05 23:04:38
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv27144 Modified Files: GotoPat.cc RangeFashion.cc SafeVelocityPat.cc Log Message: - Minors revisions Index: RangeFashion.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/RangeFashion.cc,v retrieving revision 1.6 retrieving revision 1.7 diff -C2 -d -r1.6 -r1.7 *** RangeFashion.cc 4 Jul 2005 11:21:11 -0000 1.6 --- RangeFashion.cc 5 Jul 2005 23:04:29 -0000 1.7 *************** *** 65,68 **** --- 65,88 ---- * @parameter_value 0 * + * @parameter_name MAX_DIST + * @parameter_description Distance when no data from sensor (mm) + * @parameter_type int + * @parameter_value 10000 + * + * @parameter_name MAX_DIST_IR + * @parameter_description Max range detection of IR (mm) + * @parameter_type int + * @parameter_value 1000 + * + * @parameter_name MAX_DIST_SONAR + * @parameter_description Max range detection of sonar (mm) + * @parameter_type int + * @parameter_value 10000 + * + * @parameter_name MAX_DIST_LASER + * @parameter_description Max range detection of laser (mm) + * @parameter_type int + * @parameter_value 10000 + * END*/ *************** *** 80,83 **** --- 100,107 ---- //parameters int m_typeFashion; + int m_maxDist; + int m_maxDistIR; + int m_maxDistSonar; + int m_maxDistLaser; void priorityBaseFashion(Vector<int> *vectPercept, ObjectRef &laserValue, ObjectRef &sonarValue, ObjectRef &irValue, int count){ *************** *** 95,99 **** int i=0; for(iterIR = irReading.begin(); iterIR != irReading.end(); ++iterIR){ ! if(*iterIR <= 1000) // Max value IR (*vectPercept)[i]=*iterIR; ++i; --- 119,123 ---- int i=0; for(iterIR = irReading.begin(); iterIR != irReading.end(); ++iterIR){ ! if(*iterIR <= m_maxDistIR) // Max value IR (*vectPercept)[i]=*iterIR; ++i; *************** *** 124,128 **** // Get the 3 mins of the range ! int min1=10000, min2=10000, min3=10000; for(int j=begin; j<end; ++j){ if(laserReading[j] <= min1){ --- 148,152 ---- // Get the 3 mins of the range ! int min1=m_maxDist, min2=m_maxDist, min3=m_maxDist; for(int j=begin; j<end; ++j){ if(laserReading[j] <= min1){ *************** *** 156,160 **** // Save the value ! if(((*vectPercept)[ind_vectPercept] == -1) && (x_min < 10000)){ (*vectPercept)[ind_vectPercept] = x_min - offset; } --- 180,184 ---- // Save the value ! if(((*vectPercept)[ind_vectPercept] == -1) && (x_min < m_maxDistLaser)){ (*vectPercept)[ind_vectPercept] = x_min - offset; } *************** *** 182,186 **** int i=0; for(iterSonar = sonarReading.begin(); iterSonar != sonarReading.end(); ++iterSonar) { ! if(((*vectPercept)[i] == -1) && (*iterSonar<10000)) // Max value Sonar (*vectPercept)[i]=*iterSonar; ++i; --- 206,210 ---- int i=0; for(iterSonar = sonarReading.begin(); iterSonar != sonarReading.end(); ++iterSonar) { ! if(((*vectPercept)[i] == -1) && (*iterSonar<m_maxDistSonar)) // Max value Sonar (*vectPercept)[i]=*iterSonar; ++i; *************** *** 198,202 **** for(iterPercept = vectPercept->begin(); iterPercept != vectPercept->end(); ++iterPercept) { if(*iterPercept < 0) ! *iterPercept = 10000; } --- 222,226 ---- for(iterPercept = vectPercept->begin(); iterPercept != vectPercept->end(); ++iterPercept) { if(*iterPercept < 0) ! *iterPercept = m_maxDist; } *************** *** 223,227 **** // parameters ! m_typeFashion = dereference_cast<int>(parameters.get("TYPE_FASHION")); } --- 247,255 ---- // parameters ! m_typeFashion = dereference_cast<int>(parameters.get("TYPE_FASHION")); ! m_maxDist = dereference_cast<int>(parameters.get("MAX_DIST")); ! m_maxDistIR = dereference_cast<int>(parameters.get("MAX_DIST_IR")); ! m_maxDistSonar = dereference_cast<int>(parameters.get("MAX_DIST_SONAR")); ! m_maxDistLaser = dereference_cast<int>(parameters.get("MAX_DIST_LASER")); } Index: SafeVelocityPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/SafeVelocityPat.cc,v retrieving revision 1.6 retrieving revision 1.7 diff -C2 -d -r1.6 -r1.7 *** SafeVelocityPat.cc 4 Jul 2005 11:21:11 -0000 1.6 --- SafeVelocityPat.cc 5 Jul 2005 23:04:29 -0000 1.7 *************** *** 111,114 **** --- 111,123 ---- float m_b; + float getMinMargin(const Vector<float> &vector, int begin, int end){ + float margin = vector[begin]; + for(int i = begin+1; i<=end; ++i){ + if(vector[i] < margin){ + margin = vector[i]; + } + } + } + public: *************** *** 171,209 **** } ! // According to the side of the rotation evaluation de margin of safety int rotation = dereference_cast<int>(rotationRef); float marginFront=1, marginBack=1; ! if (rotation < 0){ ! // Margin at the back of the robot (range 6,7,8,9) ! marginBack = securityMargin[6]; ! for(int i = 7; i<=9; ++i){ ! if(securityMargin[i] < marginBack){ ! marginBack = securityMargin[i]; ! } } ! ! // Margin at front of the robot (range 0,1,2,3) ! marginFront = securityMargin[0]; ! for(int i = 1; i<=3; ++i){ ! if(securityMargin[i] < marginFront){ ! marginFront = securityMargin[i]; ! } } } else{ ! // Margin at the back of the robot (range 3,4,5,6) ! marginBack = securityMargin[3]; ! for(int i = 4; i<=6; ++i){ ! if(securityMargin[i] < marginBack){ ! marginBack = securityMargin[i]; ! } } ! ! // Margin at front of the robot (range 9,10,11,0) ! marginFront = securityMargin[0]; ! for(int i = 9; i<=11; ++i){ ! if(securityMargin[i] < marginFront){ ! marginFront = securityMargin[i]; ! } } } --- 180,215 ---- } ! // According to the side of the rotation evaluate the margin of security int rotation = dereference_cast<int>(rotationRef); float marginFront=1, marginBack=1; ! // Backward or forward motion ! if(signVelIn > 0){ ! if (rotation < 0){ ! // Back margin of the robot (range 6,7,8,9) ! marginBack = getMinMargin(securityMargin, 6, 9); ! // Front margin of the robot (range 0,1,2,3) ! marginFront = getMinMargin(securityMargin, 0, 3); } ! else{ ! // Back margin of the robot (range 3,4,5,6) ! marginBack = getMinMargin(securityMargin, 3, 6); ! // Front margin of the robot (range 9,10,11,0) ! marginFront = getMinMargin(securityMargin, 9, 11); ! if(securityMargin[0] < marginFront) marginFront = securityMargin[0]; } } else{ ! if (rotation < 0){ ! // Back margin of the robot (range 3,4,5,6) ! marginBack = getMinMargin(securityMargin, 3, 6); ! // Front margin of the robot (range 9,10,11,0) ! marginFront = getMinMargin(securityMargin, 9, 11); ! if(securityMargin[0] < marginFront) marginFront = securityMargin[0]; } ! else{ ! // Back margin of the robot (range 6,7,8,9) ! marginBack = getMinMargin(securityMargin, 6, 9); ! // Front margin of the robot (range 0,1,2,3) ! marginFront = getMinMargin(securityMargin, 0, 3); } } Index: GotoPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/GotoPat.cc,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** GotoPat.cc 2 Jul 2005 02:12:43 -0000 1.4 --- GotoPat.cc 5 Jul 2005 23:04:29 -0000 1.5 *************** *** 108,112 **** * @parameter_value 3 * - * * @parameter_name DISCARD_DIST_INTER_GOAL * @parameter_description Distance to discard intermediate goal --- 108,111 ---- |
|
From: Fernyqc <fe...@us...> - 2005-07-04 22:52:15
|
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; ! } } } |
|
From: MagellanPro <mag...@us...> - 2005-07-04 11:23:49
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv13289 Modified Files: AvoidPat.cc Log Message: Speed factor modification Index: AvoidPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidPat.cc,v retrieving revision 1.6 retrieving revision 1.7 diff -C2 -d -r1.6 -r1.7 *** AvoidPat.cc 4 Jul 2005 11:21:11 -0000 1.6 --- AvoidPat.cc 4 Jul 2005 11:23:24 -0000 1.7 *************** *** 160,164 **** case 11: max_speed *= 0.85; ! min_speed *= 0.85; break; case 2: --- 160,164 ---- case 11: max_speed *= 0.85; ! min_speed *= 0.90; break; case 2: *************** *** 166,176 **** 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; } --- 166,176 ---- case 8: case 10: ! max_speed *= 0.70; ! min_speed *= 0.75; break; case 3: case 9: ! max_speed *= 0.55; ! min_speed *= 0.60; break; } |
|
From: Fernyqc <fe...@us...> - 2005-07-04 11:21:20
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv11892 Modified Files: AvoidPat.cc RangeFashion.cc SafeVelocityPat.cc Log Message: AvoidPat and RangeFashion -> parameters modifications SafeVelocity -> Allow backward motion Index: RangeFashion.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/RangeFashion.cc,v retrieving revision 1.5 retrieving revision 1.6 diff -C2 -d -r1.5 -r1.6 *** RangeFashion.cc 3 Jul 2005 04:35:47 -0000 1.5 --- RangeFashion.cc 4 Jul 2005 11:21:11 -0000 1.6 *************** *** 41,57 **** * @input_description Behavior activation * ! * @input_name LASERS * @input_type Vector<int> * @input_description All lasers reading. * ! * @input_name SONARS * @input_type Vector<int> * @input_description All sonars reading. * ! * @input_name IRS * @input_type Vector<int> * @input_description All IRs reading. * ! * @output_name PERCEPT * @output_type Vector<int> * @output_description Percept after fusion --- 41,57 ---- * @input_description Behavior activation * ! * @input_name LASER * @input_type Vector<int> * @input_description All lasers reading. * ! * @input_name SONAR * @input_type Vector<int> * @input_description All sonars reading. * ! * @input_name IR * @input_type Vector<int> * @input_description All IRs reading. * ! * @output_name RANGE_BELT * @output_type Vector<int> * @output_description Percept after fusion *************** *** 215,224 **** //inputs ! laserID = addInput("LASERS"); ! sonarID = addInput("SONARS"); ! irID = addInput("IRS"); //outputs ! perceptID = addOutput("PERCEPT"); // parameters --- 215,224 ---- //inputs ! laserID = addInput("LASER"); ! sonarID = addInput("SONAR"); ! irID = addInput("IR"); //outputs ! perceptID = addOutput("RANGE_BELT"); // parameters Index: SafeVelocityPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/SafeVelocityPat.cc,v retrieving revision 1.5 retrieving revision 1.6 diff -C2 -d -r1.5 -r1.6 *** SafeVelocityPat.cc 3 Jul 2005 23:54:19 -0000 1.5 --- SafeVelocityPat.cc 4 Jul 2005 11:21:11 -0000 1.6 *************** *** 146,153 **** // Determine the security distance int velIn = dereference_cast<int>(velInRef); float securityDist = 0; ! if(abs(velIn) < m_min_vel) securityDist = m_min_distance; ! else securityDist = m_slope*abs(velIn) + m_b; // Evaluate the security margin around the robot --- 146,156 ---- // Determine the security distance int velIn = dereference_cast<int>(velInRef); + int signVelIn = (velIn<0)?-1:1; + velIn = abs(velIn); + float securityDist = 0; ! if(velIn < m_min_vel) securityDist = m_min_distance; ! else securityDist = m_slope*velIn + m_b; // Evaluate the security margin around the robot *************** *** 213,216 **** --- 216,225 ---- if((velOut < m_min_vel) && (velIn > m_min_vel)) velOut = m_min_vel; + + // Saturate the speed output (security) + if(velOut > m_max_vel) velOut = m_max_vel; + + // Backward/Forward + velOut *= signVelIn; // Debugging purpose only Index: AvoidPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidPat.cc,v retrieving revision 1.5 retrieving revision 1.6 diff -C2 -d -r1.5 -r1.6 *** AvoidPat.cc 4 Jul 2005 03:36:24 -0000 1.5 --- AvoidPat.cc 4 Jul 2005 11:21:11 -0000 1.6 *************** *** 40,52 **** * @input_description Behavior activation * ! * @input_name DIST_OBSTACLE * @input_type Vector<int> ! * @input_description Distance of the obstacle to the robot * ! * @input_name VELOCITY_IN * @input_type int * @input_description Actual velocity of the robot * ! * @output_name VELOCITY_OUT * @output_type int * @output_description Velocity value --- 40,52 ---- * @input_description Behavior activation * ! * @input_name RANGE_BELT * @input_type Vector<int> ! * @input_description Range mesure around the robot * ! * @input_name ACTUAL_VELOCITY * @input_type int * @input_description Actual velocity of the robot * ! * @output_name VELOCITY * @output_type int * @output_description Velocity value *************** *** 188,196 **** //inputs ! rangeID = addInput("DIST_OBSTACLE"); ! velocityInID = addInput("VELOCITY_IN"); //outputs ! velocityOutID = addOutput("VELOCITY_OUT"); rotationID = addOutput("ROTATION"); --- 188,196 ---- //inputs ! rangeID = addInput("RANGE_BELT"); ! velocityInID = addInput("ACTUAL_VELOCITY"); //outputs ! velocityOutID = addOutput("VELOCITY"); rotationID = addOutput("ROTATION"); |
|
From: Fernyqc <fe...@us...> - 2005-07-04 06:47:42
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv21713 Modified Files: Makefile.am Added Files: BumperStall.cc Log Message: - BumperStall behavior - Adding BumperStall.cc to the Makefile.am Index: Makefile.am =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/Makefile.am,v retrieving revision 1.18 retrieving revision 1.19 diff -C2 -d -r1.18 -r1.19 *** Makefile.am 3 Jul 2005 04:35:47 -0000 1.18 --- Makefile.am 4 Jul 2005 06:47:32 -0000 1.19 *************** *** 35,39 **** RangeFashion.cc \ SafeVelocityPat.cc \ ! AvoidPat.cc libBehaviors_la_LDFLAGS = -release $(LT_RELEASE) --- 35,40 ---- RangeFashion.cc \ SafeVelocityPat.cc \ ! AvoidPat.cc \ ! BumperStall.cc libBehaviors_la_LDFLAGS = -release $(LT_RELEASE) --- NEW FILE: BumperStall.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 _BUMPER_STALL_CC_ #define _BUMPER_STALL_CC_ #include "Behavior.h" using namespace std; using namespace FD; namespace RobotFlow { class BumperStall; DECLARE_NODE(BumperStall) REGISTER_BEHAVIOR(BumperStall) /*Node * @name BumperStall * @category RobotFlow:Behaviors * @description No description available * * @input_name NBBUMPERS * @input_type int * @input_description Number of bumpers * * @input_name BUMPERSTATE * @input_type int * @input_description Global value of the bumpers state * * @input_name ACTIVATED * @input_type bool * @input_description Behavior activation * * @output_name VELOCITY * @output_type int * @output_description Velocity value * * @output_name ROTATION * @output_type int * @output_description Velocity value * * @output_name EXPLOITATION * @output_description Behavior exploitation * * @parameter_name BEHAVIOR_TYPE * @parameter_type int * @parameter_value 0 * @parameter_description Type of action to do on bumper contact * END*/ class BumperStall : public Behavior { //inputs int nbID; int stateID; //outputs int rotationID; int velocityID; //parameters int m_type; public: BumperStall(string nodeName, ParameterSet params) : Behavior(nodeName, params, "BumperStall") { //inputs nbID = addInput("NBBUMPERS"); stateID = addInput("BUMPERSTATE"); //outputs rotationID = addOutput("ROTATION"); velocityID = addOutput("VELOCITY"); // Parameters m_type = dereference_cast<int> (parameters.get("BEHAVIOR_TYPE")); } void calculate_behavior(int output_id, int count, Buffer &out) { ObjectRef nbRef = getInput(nbID,count); ObjectRef stateRef = getInput(stateID,count); if (!nbRef->isNil() && !stateRef->isNil()){ // Get inputs int nb = dereference_cast<int>(nbRef); int state = dereference_cast<int>(stateRef); // Apply selected behavior bool nilOutputs = true; int rotation = 0; int velocity = 0; switch(m_type){ case 0: // Stall the robot if a bumper is pressed nilOutputs = (state == 0); break; default: std::cout << "BumperStall -> Unknown behavior type" << std::endl; } // Outputs the results if(nilOutputs){ (*outputs[velocityID].buffer)[count] = nilObject; (*outputs[rotationID].buffer)[count] = nilObject; } else{ (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(velocity)); (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(rotation)); } } else { //invalid outputing nothing (*outputs[velocityID].buffer)[count] = nilObject; (*outputs[rotationID].buffer)[count] = nilObject; } }//calculate_behavior }; }//namespace RobotFlow #endif |
|
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; } } |
|
From: MagellanPro <mag...@us...> - 2005-07-03 23:54:28
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv20165 Modified Files: AvoidPat.cc SafeVelocityPat.cc Log Message: - Modification of the way rotation and velocity are calculated - Index: SafeVelocityPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/SafeVelocityPat.cc,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** SafeVelocityPat.cc 3 Jul 2005 17:39:21 -0000 1.4 --- SafeVelocityPat.cc 3 Jul 2005 23:54:19 -0000 1.5 *************** *** 149,153 **** if(abs(velIn) < m_min_vel) securityDist = m_min_distance; ! else securityDist = m_slope*velIn + m_b; // Evaluate the security margin around the robot --- 149,153 ---- if(abs(velIn) < m_min_vel) securityDist = m_min_distance; ! else securityDist = m_slope*abs(velIn) + m_b; // Evaluate the security margin around the robot Index: AvoidPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidPat.cc,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** AvoidPat.cc 3 Jul 2005 17:39:21 -0000 1.3 --- AvoidPat.cc 3 Jul 2005 23:54:19 -0000 1.4 *************** *** 140,145 **** // Slope of the line used to determine security distance ! float m_slope; ! float m_b; public: --- 140,148 ---- // Slope of the line used to determine security distance ! float m_dist_slope; ! float m_dist_b; ! ! float m_vel_slope; ! float m_vel_b; public: *************** *** 169,174 **** 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; ! m_b = m_max_distance - m_slope * m_max_robot_vel; } --- 172,180 ---- 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) / (4.0); ! m_vel_b = m_min_avoid_vel; } *************** *** 189,193 **** if(abs(velIn) < m_min_avoid_vel) securityDist = m_min_distance; ! else securityDist = m_slope*velIn + m_b; // Evaluate the security margin around the robot --- 195,199 ---- 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 *************** *** 221,226 **** 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 --- 227,232 ---- 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 *************** *** 228,232 **** // Using different relation depending of the closest obstacle to the robot ! float slope; float b; switch (ind_min_margin){ --- 234,238 ---- // Using different relation depending of the closest obstacle to the robot ! /*float slope; float b; switch (ind_min_margin){ *************** *** 305,309 **** if(ind_min_margin==8) rotation *= -1; break; ! } // Define FD output --- 311,333 ---- 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 *************** *** 325,329 **** 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; --- 349,353 ---- 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; |
|
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: |
|
From: Fernyqc <fe...@us...> - 2005-07-03 07:24:28
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv23722 Modified Files: AvoidPat.cc SafeVelocityPat.cc Log Message: - Ajusting speed control in both behavior (extra parameters) Index: SafeVelocityPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/SafeVelocityPat.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** SafeVelocityPat.cc 3 Jul 2005 04:35:47 -0000 1.2 --- SafeVelocityPat.cc 3 Jul 2005 07:24:17 -0000 1.3 *************** *** 43,47 **** * @input_description Behavior activation. * ! * @input_name VELOCITY_IN * @input_type int * @input_description Desired velocity of the robot --- 43,47 ---- * @input_description Behavior activation. * ! * @input_name DESIRED_VELOCITY * @input_type int * @input_description Desired velocity of the robot *************** *** 51,59 **** * @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 --- 51,59 ---- * @input_description Desired rotation of the robot * ! * @input_name RANGE_BELT * @input_type Vector<int> * @input_description Distance of the obstacle to the robot * ! * @output_name VELOCITY * @output_type int * @output_description Output velocity *************** *** 64,78 **** * @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) * --- 64,83 ---- * @parameter_name MAX_VELOCITY * @parameter_type int ! * @parameter_value 300 * @parameter_description Maximum velocity allowed (mm/s). * + * @parameter_name MIN_VELOCITY + * @parameter_type int + * @parameter_value 75 + * @parameter_description Minimum velocity to beat friction (mm/s). + * * @parameter_name MAX_DISTANCE * @parameter_type int ! * @parameter_value 800 * @parameter_description Maximum distance of the protection (mm) * * @parameter_name MIN_DISTANCE * @parameter_type int ! * @parameter_value 150 * @parameter_description Maximum distance of the protection (mm) * *************** *** 83,100 **** //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: --- 88,107 ---- //inputs ! int desiredVelID; int rotationID; int rangeID; //outputs ! int velocityID; //parameters int m_max_vel; + int m_min_vel; int m_max_distance; int m_min_distance; // Slope of the line used to determine security distance ! float m_slope; ! float m_b; public: *************** *** 104,124 **** //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); --- 111,133 ---- //inputs ! desiredVelID = addInput("DESIRED_VELOCITY"); rotationID = addInput("ROTATION"); ! rangeID = addInput("RANGE_BELT"); //outputs ! velocityID = addOutput("VELOCITY"); // Parameters m_max_vel = dereference_cast<int> (parameters.get("MAX_VELOCITY")); + m_min_vel = dereference_cast<int> (parameters.get("MIN_VELOCITY")); m_max_distance = dereference_cast<int> (parameters.get("MAX_DISTANCE")); m_min_distance = dereference_cast<int> (parameters.get("MIN_DISTANCE")); ! m_slope = (1.0*m_max_distance - m_min_distance) / (m_max_vel - m_min_vel); ! m_b = m_max_distance - m_slope * m_max_vel; } virtual void calculate_behavior(int output_id, int count, Buffer &out) { ! ObjectRef velInRef = getInput(desiredVelID,count); ObjectRef rotationRef = getInput(rotationID,count); ObjectRef rangeValueRef = getInput(rangeID,count); *************** *** 130,134 **** // Determine the security distance int velIn = dereference_cast<int>(velInRef); ! float securityDist = m*velIn + m_min_distance; // Evaluate the security margin around the robot --- 139,146 ---- // Determine the security distance 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; // Evaluate the security margin around the robot *************** *** 187,190 **** --- 199,205 ---- velOut = static_cast<int>(velIn * marginFront); else velOut = static_cast<int>(velIn * marginBack); + + if((velOut < m_min_vel) && (velIn > m_min_vel)) + velOut = m_min_vel; // Debugging purpose only *************** *** 207,211 **** } ! (*outputs[velocityOutID].buffer)[count] = ObjectRef(Int::alloc(velOut)); }//calculate_behavior --- 222,226 ---- } ! (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(velOut)); }//calculate_behavior Index: AvoidPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidPat.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** AvoidPat.cc 3 Jul 2005 04:35:46 -0000 1.1 --- AvoidPat.cc 3 Jul 2005 07:24:17 -0000 1.2 *************** *** 59,70 **** * @output_description Behavior exploitation. * ! * @parameter_name MAX_VELOCITY * @parameter_type int ! * @parameter_value 300 * @parameter_description Maximum velocity allowed (mm/s). * * @parameter_name MAX_ROTATION * @parameter_type int ! * @parameter_value 25 * @parameter_description Maximum rotation allowed (degree/s). * --- 59,75 ---- * @output_description Behavior exploitation. * ! * @parameter_name MAX_AVOID_VELOCITY * @parameter_type int ! * @parameter_value 200 * @parameter_description Maximum velocity allowed (mm/s). * + * @parameter_name MIN_AVOID_VELOCITY + * @parameter_type int + * @parameter_value 75 + * @parameter_description Minimum velocity to help the rotation (mm/s). + * * @parameter_name MAX_ROTATION * @parameter_type int ! * @parameter_value 15 * @parameter_description Maximum rotation allowed (degree/s). * *************** *** 72,76 **** * @parameter_type int * @parameter_value 5 ! * @parameter_description Maximum rotation allowed (degree/s). * * @parameter_name MIN_ROTATION_GAIN --- 77,81 ---- * @parameter_type int * @parameter_value 5 ! * @parameter_description Minimum rotation to get something going (degree/s). * * @parameter_name MIN_ROTATION_GAIN *************** *** 78,95 **** * @parameter_value 1.5 * @parameter_description Gain to apply to the MIN_ROTATION value ! * * @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 300 ! * @parameter_description Maximum distance of the protection (mm) * * @parameter_name DIST_BACKWARD * @parameter_type int ! * @parameter_value 300 * @parameter_description Distance to go backward * --- 83,105 ---- * @parameter_value 1.5 * @parameter_description Gain to apply to the MIN_ROTATION value ! * ! * @parameter_name MAX_ROBOT_VELOCITY ! * @parameter_type int ! * @parameter_value 300 ! * @parameter_description Maximum distance of the protection (mm) ! * * @parameter_name MAX_DISTANCE * @parameter_type int ! * @parameter_value 600 * @parameter_description Maximum distance of the protection (mm) * * @parameter_name MIN_DISTANCE * @parameter_type int ! * @parameter_value 150 ! * @parameter_description Minimum distance of the protection (mm) * * @parameter_name DIST_BACKWARD * @parameter_type int ! * @parameter_value 200 * @parameter_description Distance to go backward * *************** *** 108,112 **** //parameters ! int m_max_vel; int m_max_rot; int m_min_rot; --- 118,124 ---- //parameters ! int m_max_avoid_vel; ! int m_min_avoid_vel; ! int m_max_robot_vel; int m_max_rot; int m_min_rot; *************** *** 117,121 **** // Slope of the line used to determine security distance ! float m_vel_m; public: --- 129,134 ---- // Slope of the line used to determine security distance ! float m_slope; ! float m_b; public: *************** *** 133,137 **** // Parameters ! m_max_vel = dereference_cast<int> (parameters.get("MAX_VELOCITY")); m_max_rot = dereference_cast<int> (parameters.get("MAX_ROTATION")); m_min_rot = dereference_cast<int> (parameters.get("MIN_ROTATION")); --- 146,152 ---- // 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")); *************** *** 141,145 **** m_dist_backward = dereference_cast<int> (parameters.get("DIST_BACKWARD")); ! m_vel_m = (1.0*m_max_distance - m_min_distance) /m_max_vel; } --- 156,161 ---- m_dist_backward = dereference_cast<int> (parameters.get("DIST_BACKWARD")); ! m_slope = (1.0*m_max_distance - m_min_distance) / m_max_robot_vel; ! m_b = m_max_distance - m_slope * m_max_robot_vel; } *************** *** 151,161 **** // Get the actual velocity of the robot ! int velIn = m_max_vel; ! /*if(!velInRef->isNil()){ velIn = dereference_cast<int>(velInRef); ! }*/ // Determine the security distance ! float securityDist = m_vel_m*velIn + m_min_distance; // Evaluate the security margin around the robot --- 167,180 ---- // 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(velIn < m_min_avoid_vel) ! securityDist = m_min_distance; ! else securityDist = m_slope*velIn + m_b; // Evaluate the security margin around the robot *************** *** 184,188 **** if(min_margin < 1){ // Determine linear speed according to the security margin ! int velOut = static_cast<int>(m_max_vel * min_margin); // Determine rotational speed --- 203,208 ---- 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 |
|
From: Fernyqc <fe...@us...> - 2005-07-03 04:35:57
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv8542 Modified Files: Makefile.am RangeFashion.cc SafeVelocityPat.cc Added Files: AvoidPat.cc Log Message: - First version of AvoidPat; - Addition of AvoidPat.cc to the Makefile.am; - The speed management in SafeVelocity has been reviewed; - Indentation modification in RangeFashion; Index: Makefile.am =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/Makefile.am,v retrieving revision 1.17 retrieving revision 1.18 diff -C2 -d -r1.17 -r1.18 *** Makefile.am 2 Jul 2005 02:12:43 -0000 1.17 --- Makefile.am 3 Jul 2005 04:35:47 -0000 1.18 *************** *** 34,38 **** FollowWall.cc \ RangeFashion.cc \ ! SafeVelocityPat.cc libBehaviors_la_LDFLAGS = -release $(LT_RELEASE) --- 34,39 ---- FollowWall.cc \ RangeFashion.cc \ ! SafeVelocityPat.cc \ ! AvoidPat.cc libBehaviors_la_LDFLAGS = -release $(LT_RELEASE) Index: RangeFashion.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/RangeFashion.cc,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** RangeFashion.cc 2 Jul 2005 04:05:46 -0000 1.4 --- RangeFashion.cc 3 Jul 2005 04:35:47 -0000 1.5 *************** *** 146,169 **** int offset = 0; switch (ind_vectPercept){ ! case 0: offset = 100; break; ! case 1: offset = 150; break; ! case 2: offset = 150; break; ! case 3: offset = 300; break; ! case 9: offset = 300; break; ! case 10: offset = 150; break; ! case 11: offset = 150; break; } ! // Save the value ! if(((*vectPercept)[ind_vectPercept] == -1) && (x_min < 10000)){ ! (*vectPercept)[ind_vectPercept] = x_min - offset; } ! // Next position ! --ind_vectPercept; ! if(ind_vectPercept<0) ! ind_vectPercept = 11; } - } --- 146,168 ---- int offset = 0; switch (ind_vectPercept){ ! case 0: offset = 100; break; ! case 1: offset = 150; break; ! case 2: offset = 150; break; ! case 3: offset = 300; break; ! case 9: offset = 300; break; ! case 10: offset = 150; break; ! case 11: offset = 150; break; } ! // Save the value ! if(((*vectPercept)[ind_vectPercept] == -1) && (x_min < 10000)){ ! (*vectPercept)[ind_vectPercept] = x_min - offset; } ! // Next position ! --ind_vectPercept; ! if(ind_vectPercept<0) ! ind_vectPercept = 11; } } Index: SafeVelocityPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/SafeVelocityPat.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** SafeVelocityPat.cc 2 Jul 2005 02:12:43 -0000 1.1 --- SafeVelocityPat.cc 3 Jul 2005 04:35:47 -0000 1.2 *************** *** 101,105 **** SafeVelocityPat(string nodeName, ParameterSet params) ! : Behavior(nodeName, params, "SafeVelocity") { //inputs --- 101,105 ---- SafeVelocityPat(string nodeName, ParameterSet params) ! : Behavior(nodeName, params, "SafeVelocityPat") { //inputs *************** *** 129,133 **** // Determine the security distance - cout << "castVel" << std::endl; int velIn = dereference_cast<int>(velInRef); float securityDist = m*velIn + m_min_distance; --- 129,132 ---- *************** *** 138,142 **** iterMargin = securityMargin.begin(); - cout << "castRange" << std::endl; Vector<int> &rangeValue = object_cast<Vector<int> >(getInput(rangeID,count)); Vector<int>::iterator iterRange; --- 137,140 ---- *************** *** 147,193 **** } ! // 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; --- 145,193 ---- } ! // According to the side of the rotation evaluation de margin of safety int rotation = dereference_cast<int>(rotationRef); ! float marginFront=1, marginBack=1; if (rotation < 0){ ! // Margin at the back of the robot (range 6,7,8,9) ! marginBack = securityMargin[6]; ! for(int i = 7; i<=9; ++i){ ! if(securityMargin[i] < marginBack){ ! marginBack = securityMargin[i]; ! } ! } ! // Margin at front of the robot (range 0,1,2,3) ! marginFront = securityMargin[0]; ! for(int i = 1; i<=3; ++i){ ! if(securityMargin[i] < marginFront){ ! marginFront = securityMargin[i]; ! } ! } } else{ ! // Margin at the back of the robot (range 3,4,5,6) ! marginBack = securityMargin[3]; ! for(int i = 4; i<=6; ++i){ ! if(securityMargin[i] < marginBack){ ! marginBack = securityMargin[i]; ! } ! } ! // Margin at front of the robot (range 9,10,11,0) ! marginFront = securityMargin[0]; ! for(int i = 9; i<=11; ++i){ ! if(securityMargin[i] < marginFront){ ! marginFront = securityMargin[i]; ! } ! } } ! // Adjust the speed according to the margins ! if(marginFront < marginBack) ! velOut = static_cast<int>(velIn * marginFront); ! else velOut = static_cast<int>(velIn * marginBack); // Debugging purpose only + std::cout << "Safe Velocity" << std::endl; if(velOut != velIn) std::cout << "(velOut != velIn)" << std::endl; *************** *** 201,209 **** 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; } --- 201,208 ---- std::cout << *iterMargin << " "; std::cout << std::endl; ! std::cout << "marginFront -> " << marginFront << std::endl; ! std::cout << "marginBack -> " << marginBack << std::endl; std::cout << "velIn -> " << velIn << std::endl; std::cout << "velOut -> " << velOut << std::endl; } --- NEW FILE: AvoidPat.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 _AVOID_PAT_CC_ #define _AVOID_PAT_CC_ #include "Behavior.h" using namespace std; using namespace FD; namespace RobotFlow { class AvoidPat; DECLARE_NODE(AvoidPat) REGISTER_BEHAVIOR(AvoidPat) /*Node * @name AvoidPat * @category RobotFlow:Behaviors * @description No description available * * @input_name ACTIVATED * @input_type bool * @input_description Behavior activation * * @input_name DIST_OBSTACLE * @input_type Vector<int> * @input_description Distance of the obstacle to the robot * * @input_name VELOCITY_IN * @input_type int * @input_description Actual velocity of the robot * * @output_name VELOCITY_OUT * @output_type int * @output_description Velocity value * * @output_name ROTATION * @output_type int * @output_description Rotation value * * @output_name EXPLOITATION * @output_description Behavior exploitation. * * @parameter_name MAX_VELOCITY * @parameter_type int * @parameter_value 300 * @parameter_description Maximum velocity allowed (mm/s). * * @parameter_name MAX_ROTATION * @parameter_type int * @parameter_value 25 * @parameter_description Maximum rotation allowed (degree/s). * * @parameter_name MIN_ROTATION * @parameter_type int * @parameter_value 5 * @parameter_description Maximum rotation allowed (degree/s). * * @parameter_name MIN_ROTATION_GAIN * @parameter_type float * @parameter_value 1.5 * @parameter_description Gain to apply to the MIN_ROTATION value * * @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 300 * @parameter_description Maximum distance of the protection (mm) * * @parameter_name DIST_BACKWARD * @parameter_type int * @parameter_value 300 * @parameter_description Distance to go backward * END*/ class AvoidPat : public Behavior { //inputs int rangeID; int velocityInID; //outputs int velocityOutID; int rotationID; //parameters int m_max_vel; int m_max_rot; int m_min_rot; float m_min_rot_gain; int m_max_distance; int m_min_distance; int m_dist_backward; // Slope of the line used to determine security distance float m_vel_m; public: AvoidPat(string nodeName, ParameterSet params) : Behavior(nodeName, params, "AvoidPat") { //inputs rangeID = addInput("DIST_OBSTACLE"); velocityInID = addInput("VELOCITY_IN"); //outputs velocityOutID = addOutput("VELOCITY_OUT"); rotationID = addOutput("ROTATION"); // Parameters m_max_vel = dereference_cast<int> (parameters.get("MAX_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_dist_backward = dereference_cast<int> (parameters.get("DIST_BACKWARD")); m_vel_m = (1.0*m_max_distance - m_min_distance) /m_max_vel; } void calculate_behavior(int output_id, int count, Buffer &out) { ObjectRef rangeValueRef = getInput(rangeID,count); ObjectRef velInRef = getInput(velocityInID,count); if(!rangeValueRef->isNil()){ // Get the actual velocity of the robot int velIn = m_max_vel; /*if(!velInRef->isNil()){ velIn = dereference_cast<int>(velInRef); }*/ // Determine the security distance float securityDist = m_vel_m*velIn + m_min_distance; // 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; for(iterRange = rangeValue.begin(); iterRange != rangeValue.end(); ++iterRange){ *iterMargin = 1 + (*iterRange - 1.0*securityDist) / securityDist; if(*iterMargin > 1) *iterMargin = 1; ++iterMargin; } // 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_vel * min_margin); // 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 } 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; /*} 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; } // 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 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.size() -> " << securityMargin.size() << 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 << "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{ // 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; } } else{ (*outputs[velocityOutID].buffer)[count] = nilObject; (*outputs[rotationID].buffer)[count] = nilObject; } }//calculate_behavior }; }//namespace RobotFlow #endif |
|
From: MagellanPro <mag...@us...> - 2005-07-02 04:06:00
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv15437 Modified Files: RangeFashion.cc Log Message: - Transform on the laser data Index: RangeFashion.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/RangeFashion.cc,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** RangeFashion.cc 2 Jul 2005 02:12:43 -0000 1.3 --- RangeFashion.cc 2 Jul 2005 04:05:46 -0000 1.4 *************** *** 143,150 **** int x_min = (min1+min2+min3)/3; // Save the value ! if(((*vectPercept)[ind_vectPercept] == -1) && (x_min < 10000)) ! (*vectPercept)[ind_vectPercept] = x_min; ! // Next position --ind_vectPercept; --- 143,163 ---- int x_min = (min1+min2+min3)/3; + // Transform of the data according to the physical dimension of the robot + int offset = 0; + switch (ind_vectPercept){ + case 0: offset = 100; break; + case 1: offset = 150; break; + case 2: offset = 150; break; + case 3: offset = 300; break; + case 9: offset = 300; break; + case 10: offset = 150; break; + case 11: offset = 150; break; + } + // Save the value ! if(((*vectPercept)[ind_vectPercept] == -1) && (x_min < 10000)){ ! (*vectPercept)[ind_vectPercept] = x_min - offset; ! } ! // Next position --ind_vectPercept; |
|
From: Fernyqc <fe...@us...> - 2005-07-02 02:12:57
|
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; } |
|
From: Dominic L. <ma...@us...> - 2005-07-01 18:22:27
|
Update of /cvsroot/robotflow/RobotFlow/Vision/n-files In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv2732 Modified Files: ChallengeVision.n Log Message: PTZ adjusts Index: ChallengeVision.n =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/n-files/ChallengeVision.n,v retrieving revision 1.6 retrieving revision 1.7 diff -C2 -d -r1.6 -r1.7 *** ChallengeVision.n 30 Jun 2005 20:42:52 -0000 1.6 --- ChallengeVision.n 1 Jul 2005 18:22:15 -0000 1.7 *************** *** 94,98 **** <Node name="node_Subsumption_1" type="Subsumption" x="1653.000000" y="117.000000"/> <Node name="node_Subsumption_2" type="Subsumption" x="2174.000000" y="-90.000000"/> ! <Node name="node_IF_1" type="IF" x="2307.000000" y="-5.000000"> <Parameter name="PULL_ANYWAY" type="bool" value="" description="If true, the IF statement pulls also on the branch not taken"/> </Node> --- 94,98 ---- <Node name="node_Subsumption_1" type="Subsumption" x="1653.000000" y="117.000000"/> <Node name="node_Subsumption_2" type="Subsumption" x="2174.000000" y="-90.000000"/> ! <Node name="node_IF_1" type="IF" x="2326.000000" y="-5.000000"> <Parameter name="PULL_ANYWAY" type="bool" value="" description="If true, the IF statement pulls also on the branch not taken"/> </Node> *************** *** 105,109 **** <Parameter name="VALUE" type="subnet_param" value="DEFAULT_ABS_PAN_COMMAND" description="The value"/> </Node> ! <Node name="node_Constant_12" type="Constant" x="2102.000000" y="-6.000000"> <Parameter name="VALUE" type="subnet_param" value="DEFAULT_ABS_TILT_COMMAND" description="The value"/> </Node> --- 105,109 ---- <Parameter name="VALUE" type="subnet_param" value="DEFAULT_ABS_PAN_COMMAND" description="The value"/> </Node> ! <Node name="node_Constant_12" type="Constant" x="2102.000000" y="-4.000000"> <Parameter name="VALUE" type="subnet_param" value="DEFAULT_ABS_TILT_COMMAND" description="The value"/> </Node> *************** *** 248,252 **** </Node> <Node name="node_GenericPID_1" type="GenericPID" x="42.000000" y="-300.000000"> ! <Parameter name="P_GAIN" type="float" value="0.25" description="Proportionnal gain."/> <Parameter name="I_GAIN" type="float" value="0.0" description="Integral gain."/> <Parameter name="D_GAIN" type="float" value="0.0" description="Derivative gain."/> --- 248,252 ---- </Node> <Node name="node_GenericPID_1" type="GenericPID" x="42.000000" y="-300.000000"> ! <Parameter name="P_GAIN" type="float" value="0.1" description="Proportionnal gain."/> <Parameter name="I_GAIN" type="float" value="0.0" description="Integral gain."/> <Parameter name="D_GAIN" type="float" value="0.0" description="Derivative gain."/> *************** *** 263,267 **** </Node> <Node name="node_GenericPID_2" type="GenericPID" x="45.000000" y="-184.000000"> ! <Parameter name="P_GAIN" type="float" value="-0.25" description="Proportionnal gain."/> <Parameter name="I_GAIN" type="float" value="0.0" description="Integral gain."/> <Parameter name="D_GAIN" type="float" value="0.0" description="Derivative gain."/> --- 263,267 ---- </Node> <Node name="node_GenericPID_2" type="GenericPID" x="45.000000" y="-184.000000"> ! <Parameter name="P_GAIN" type="float" value="-0.1" description="Proportionnal gain."/> <Parameter name="I_GAIN" type="float" value="0.0" description="Integral gain."/> <Parameter name="D_GAIN" type="float" value="0.0" description="Derivative gain."/> *************** *** 282,288 **** <Parameter name="PULL_ANYWAY" type="bool" value="" description="If true, the IF statement pulls also on the branch not taken"/> </Node> - <Node name="node_Constant_10" type="Constant" x="903.000000" y="115.000000"> - <Parameter name="VALUE" type="int" value="0" description="The value"/> - </Node> <Node name="node_IF_5" type="IF" x="666.000000" y="135.000000"> <Parameter name="PULL_ANYWAY" type="bool" value="" description="If true, the IF statement pulls also on the branch not taken"/> --- 282,285 ---- *************** *** 295,300 **** <Node name="node_NOP_5" type="NOP" x="-199.000000" y="172.000000"/> <Node name="node_NOP_6" type="NOP" x="-107.000000" y="26.000000"/> ! <Node name="node_Constant_11" type="Constant" x="-33.000000" y="127.000000"> ! <Parameter name="VALUE" type="float" value="25" description="The value"/> </Node> <Node name="node_Smaller_2" type="Smaller" x="135.000000" y="180.000000"/> --- 292,297 ---- <Node name="node_NOP_5" type="NOP" x="-199.000000" y="172.000000"/> <Node name="node_NOP_6" type="NOP" x="-107.000000" y="26.000000"/> ! <Node name="node_Constant_11" type="Constant" x="-51.000000" y="127.000000"> ! <Parameter name="VALUE" type="float" value="100" description="The value"/> </Node> <Node name="node_Smaller_2" type="Smaller" x="135.000000" y="180.000000"/> *************** *** 304,311 **** <Node name="node_Greater_3" type="Greater" x="271.000000" y="56.000000"/> <Node name="node_Constant_12" type="Constant" x="-116.000000" y="62.000000"> ! <Parameter name="VALUE" type="int" value="25" description="The value"/> </Node> <Node name="node_Constant_13" type="Constant" x="195.000000" y="267.000000"> ! <Parameter name="VALUE" type="int" value="-50" description="The value"/> </Node> <Node name="node_IF_6" type="IF" x="425.000000" y="250.000000"> --- 301,308 ---- <Node name="node_Greater_3" type="Greater" x="271.000000" y="56.000000"/> <Node name="node_Constant_12" type="Constant" x="-116.000000" y="62.000000"> ! <Parameter name="VALUE" type="int" value="15" description="The value"/> </Node> <Node name="node_Constant_13" type="Constant" x="195.000000" y="267.000000"> ! <Parameter name="VALUE" type="int" value="-300" description="The value"/> </Node> <Node name="node_IF_6" type="IF" x="425.000000" y="250.000000"> *************** *** 315,320 **** <Parameter name="VALUE" type="int" value="0" description="The value"/> </Node> ! <Node name="node_Constant_15" type="Constant" x="435.000000" y="64.000000"> ! <Parameter name="VALUE" type="int" value="50" description="The value"/> </Node> <Node name="node_ABS_1" type="ABS" x="-113.000000" y="112.000000"/> --- 312,317 ---- <Parameter name="VALUE" type="int" value="0" description="The value"/> </Node> ! <Node name="node_Constant_15" type="Constant" x="510.000000" y="136.000000"> ! <Parameter name="VALUE" type="int" value="300" description="The value"/> </Node> <Node name="node_ABS_1" type="ABS" x="-113.000000" y="112.000000"/> *************** *** 334,344 **** <Node name="node_NOP_9" type="NOP" x="514.000000" y="-219.000000"/> <Node name="node_Add_4" type="Add" x="654.000000" y="-188.000000"/> ! <Node name="node_NOP_10" type="NOP" x="102.000000" y="-235.000000"/> ! <Node name="node_AUTO_SCALE_1" type="AUTO_SCALE" x="386.000000" y="-293.000000"/> ! <Node name="node_AUTO_SCALE_2" type="AUTO_SCALE" x="384.000000" y="-176.000000"/> ! <Node name="node_Div_1" type="Div" x="555.000000" y="135.000000"/> ! <Node name="node_ZOOM_FACTOR_1" type="ZOOM_FACTOR" x="58.000000" y="242.000000"/> ! <Node name="node_Div_2" type="Div" x="313.000000" y="316.000000"/> <Node name="node_Add_5" type="Add" x="799.000000" y="128.000000"/> <Link from="node_isNil_1" output="OUTPUT" to="node_IF_1" input="COND"/> <Link from="node_Constant_1" output="VALUE" to="node_IF_1" input="THEN"/> --- 331,342 ---- <Node name="node_NOP_9" type="NOP" x="514.000000" y="-219.000000"/> <Node name="node_Add_4" type="Add" x="654.000000" y="-188.000000"/> ! <Node name="node_NOP_10" type="NOP" x="81.000000" y="-238.000000"/> <Node name="node_Add_5" type="Add" x="799.000000" y="128.000000"/> + <Node name="node_AUTO_SCALE_1" type="AUTO_SCALE" x="309.000000" y="-291.000000"/> + <Node name="node_AUTO_SCALE_2" type="AUTO_SCALE" x="307.000000" y="-178.000000"/> + <Node name="node_Sub_5" type="Sub" x="866.000000" y="56.000000"/> + <Node name="node_Constant_7" type="Constant" x="599.000000" y="13.000000"> + <Parameter name="VALUE" type="int" value="1000" description="The value"/> + </Node> <Link from="node_isNil_1" output="OUTPUT" to="node_IF_1" input="COND"/> <Link from="node_Constant_1" output="VALUE" to="node_IF_1" input="THEN"/> *************** *** 362,367 **** <Link from="node_Smaller_1" output="OUTPUT" to="node_AND_1" input="INPUT1"/> <Link from="node_Smaller_2" output="OUTPUT" to="node_AND_1" input="INPUT2">164.5 180 191 179 190 136 209.5 135.5 </Link> - <Link from="node_Constant_11" output="VALUE" to="node_Smaller_1" input="INPUT2"/> - <Link from="node_Constant_11" output="VALUE" to="node_Smaller_2" input="INPUT2">1 127 1 187 51.5 187.5 </Link> <Link from="node_Greater_3" output="OUTPUT" to="node_AND_2" input="INPUT1">300.5 56 326 56 327 113 365.5 112.5 </Link> <Link from="node_AND_1" output="OUTPUT" to="node_AND_2" input="INPUT2"/> --- 360,363 ---- *************** *** 397,401 **** <Link from="node_GetVisualROIParam_1" output="HALF_WIDTH" to="node_Sub_3" input="INPUT2">-801 -126 -774 -125 -773 -28 -708.5 -27.5 </Link> <Link from="node_isNil_3" output="OUTPUT" to="node_IF_4" input="COND">683 -91 711 -91 711 99 997 99 </Link> - <Link from="node_Constant_10" output="VALUE" to="node_IF_4" input="THEN"/> <Link from="node_Add_3" output="OUTPUT" to="node_NOP_2" input="INPUT"/> <Link from="node_ToInt_1" output="OUTPUT" to="node_Add_3" input="INPUT2"/> --- 393,396 ---- *************** *** 404,423 **** <Link from="node_NOP_9" output="OUTPUT" to="node_Add_4" input="INPUT1"/> <Link from="node_ToInt_2" output="OUTPUT" to="node_Add_4" input="INPUT2"/> - <Link from="node_AUTO_SCALE_1" output="OUTPUT" to="node_ToInt_1" input="INPUT"/> - <Link from="node_GenericPID_1" output="OUTPUT" to="node_AUTO_SCALE_1" input="INPUT"/> - <Link from="node_AUTO_SCALE_2" output="OUTPUT" to="node_ToInt_2" input="INPUT"/> - <Link from="node_GenericPID_2" output="OUTPUT" to="node_AUTO_SCALE_2" input="INPUT"/> - <Link from="node_NOP_10" output="OUTPUT" to="node_AUTO_SCALE_1" input="ABS_ZOOM"/> - <Link from="node_NOP_10" output="OUTPUT" to="node_AUTO_SCALE_2" input="ABS_ZOOM"/> - <Link from="node_Div_1" output="OUTPUT" to="node_IF_5" input="THEN"/> - <Link from="node_Constant_15" output="VALUE" to="node_Div_1" input="NUM"/> - <Link from="node_ZOOM_FACTOR_1" output="ZOOM_FACTOR" to="node_Div_1" input="DEN"/> - <Link from="node_NOP_10" output="OUTPUT" to="node_ZOOM_FACTOR_1" input="ABS_ZOOM"/> - <Link from="node_Div_2" output="OUTPUT" to="node_IF_6" input="ELSE"/> - <Link from="node_Constant_13" output="VALUE" to="node_Div_2" input="NUM"/> - <Link from="node_ZOOM_FACTOR_1" output="ZOOM_FACTOR" to="node_Div_2" input="DEN">112 243 113 323 260 323.5 </Link> <Link from="node_Add_5" output="OUTPUT" to="node_IF_4" input="ELSE"/> <Link from="node_IF_5" output="OUTPUT" to="node_Add_5" input="INPUT2"/> <Link from="node_NOP_10" output="OUTPUT" to="node_Add_5" input="INPUT1"/> <NetInput name="IN_ROI" node="node_NOP_1" terminal="INPUT" object_type="any" description="The input"/> <NetOutput name="ABS_PAN" node="node_NOP_2" terminal="OUTPUT" object_type="any" description="The output = The input"/> --- 399,418 ---- <Link from="node_NOP_9" output="OUTPUT" to="node_Add_4" input="INPUT1"/> <Link from="node_ToInt_2" output="OUTPUT" to="node_Add_4" input="INPUT2"/> <Link from="node_Add_5" output="OUTPUT" to="node_IF_4" input="ELSE"/> <Link from="node_IF_5" output="OUTPUT" to="node_Add_5" input="INPUT2"/> <Link from="node_NOP_10" output="OUTPUT" to="node_Add_5" input="INPUT1"/> + <Link from="node_NOP_10" output="OUTPUT" to="node_AUTO_SCALE_1" input="ABS_ZOOM"/> + <Link from="node_AUTO_SCALE_1" output="OUTPUT" to="node_ToInt_1" input="INPUT"/> + <Link from="node_NOP_10" output="OUTPUT" to="node_AUTO_SCALE_2" input="ABS_ZOOM"/> + <Link from="node_AUTO_SCALE_2" output="OUTPUT" to="node_ToInt_2" input="INPUT"/> + <Link from="node_GenericPID_1" output="OUTPUT" to="node_AUTO_SCALE_1" input="DELTA"/> + <Link from="node_GenericPID_2" output="OUTPUT" to="node_AUTO_SCALE_2" input="DELTA"/> + <Link from="node_NOP_10" output="OUTPUT" to="node_Sub_5" input="INPUT1"/> + <Link from="node_Constant_7" output="VALUE" to="node_Sub_5" input="INPUT2"/> + <Link from="node_Sub_5" output="OUTPUT" to="node_IF_4" input="THEN"/> + <Link from="node_Constant_13" output="VALUE" to="node_IF_6" input="ELSE"/> + <Link from="node_Constant_15" output="VALUE" to="node_IF_5" input="THEN"/> + <Link from="node_Constant_11" output="VALUE" to="node_Smaller_1" input="INPUT2"/> + <Link from="node_Constant_11" output="VALUE" to="node_Smaller_2" input="INPUT2"/> <NetInput name="IN_ROI" node="node_NOP_1" terminal="INPUT" object_type="any" description="The input"/> <NetOutput name="ABS_PAN" node="node_NOP_2" terminal="OUTPUT" object_type="any" description="The output = The input"/> *************** *** 463,483 **** <Parameter name="VALUE" type="float" value="-1" description="The value"/> </Node> ! <Node name="node_NOP_3" type="NOP" x="102.000000" y="-140.000000"/> <Node name="node_Constant_2" type="Constant" x="-358.000000" y="-96.000000"> <Parameter name="VALUE" type="float" value="1.1" description="The value"/> </Node> ! <Node name="node_Mul_2" type="Mul" x="-56.000000" y="-140.000000"/> <Node name="node_Sub_1" type="Sub" x="-483.000000" y="-52.000000"/> <Node name="node_Constant_3" type="Constant" x="-642.000000" y="-20.000000"> <Parameter name="VALUE" type="float" value="1" description="The value"/> </Node> - <Node name="node_Div_1" type="Div" x="-567.000000" y="-139.000000"/> - <Node name="node_Constant_4" type="Constant" x="-681.000000" y="-105.000000"> - <Parameter name="VALUE" type="float" value="683" description="The value"/> - </Node> - <Node name="node_Add_1" type="Add" x="-458.000000" y="-147.000000"/> - <Node name="node_Constant_5" type="Constant" x="-614.000000" y="-199.000000"> - <Parameter name="VALUE" type="float" value="1.0" description="The value"/> - </Node> <Node name="node_ZOOM_FACTOR_1" type="ZOOM_FACTOR" x="-745.000000" y="-60.000000"/> <Link from="node_Constant_2" output="VALUE" to="node_Power_1" input="BASE"/> --- 458,470 ---- <Parameter name="VALUE" type="float" value="-1" description="The value"/> </Node> ! <Node name="node_NOP_3" type="NOP" x="165.000000" y="-64.000000"/> <Node name="node_Constant_2" type="Constant" x="-358.000000" y="-96.000000"> <Parameter name="VALUE" type="float" value="1.1" description="The value"/> </Node> ! <Node name="node_Mul_2" type="Mul" x="7.000000" y="-64.000000"/> <Node name="node_Sub_1" type="Sub" x="-483.000000" y="-52.000000"/> <Node name="node_Constant_3" type="Constant" x="-642.000000" y="-20.000000"> <Parameter name="VALUE" type="float" value="1" description="The value"/> </Node> <Node name="node_ZOOM_FACTOR_1" type="ZOOM_FACTOR" x="-745.000000" y="-60.000000"/> <Link from="node_Constant_2" output="VALUE" to="node_Power_1" input="BASE"/> *************** *** 488,500 **** <Link from="node_Power_1" output="OUTPUT" to="node_Mul_2" input="INPUT2"/> <Link from="node_Constant_3" output="VALUE" to="node_Sub_1" input="INPUT2"/> - <Link from="node_Constant_4" output="VALUE" to="node_Div_1" input="DEN"/> - <Link from="node_Constant_5" output="VALUE" to="node_Add_1" input="INPUT1"/> - <Link from="node_Div_1" output="OUTPUT" to="node_Add_1" input="INPUT2"/> - <Link from="node_Add_1" output="OUTPUT" to="node_Mul_2" input="INPUT1"/> <Link from="node_NOP_2" output="OUTPUT" to="node_ZOOM_FACTOR_1" input="ABS_ZOOM"/> <Link from="node_ZOOM_FACTOR_1" output="ZOOM_FACTOR" to="node_Sub_1" input="INPUT1"/> <NetOutput name="OUTPUT" node="node_NOP_3" terminal="OUTPUT" object_type="any" description="The output = The input"/> - <NetInput name="INPUT" node="node_Div_1" terminal="NUM" object_type="any" description="The numerator"/> <NetInput name="ABS_ZOOM" node="node_NOP_2" terminal="INPUT" object_type="any" description="The input"/> <Note x="0" y="0" visible="0" text="Created with FlowDesigner 0.9.0"/> </Network> --- 475,483 ---- <Link from="node_Power_1" output="OUTPUT" to="node_Mul_2" input="INPUT2"/> <Link from="node_Constant_3" output="VALUE" to="node_Sub_1" input="INPUT2"/> <Link from="node_NOP_2" output="OUTPUT" to="node_ZOOM_FACTOR_1" input="ABS_ZOOM"/> <Link from="node_ZOOM_FACTOR_1" output="ZOOM_FACTOR" to="node_Sub_1" input="INPUT1"/> <NetOutput name="OUTPUT" node="node_NOP_3" terminal="OUTPUT" object_type="any" description="The output = The input"/> <NetInput name="ABS_ZOOM" node="node_NOP_2" terminal="INPUT" object_type="any" description="The input"/> + <NetInput name="DELTA" node="node_Mul_2" terminal="INPUT1" object_type="any" description="The first operand"/> <Note x="0" y="0" visible="0" text="Created with FlowDesigner 0.9.0"/> </Network> |
|
From: Pierre M. <sid...@us...> - 2005-07-01 17:53:51
|
Update of /cvsroot/robotflow/RobotFlow/Probes/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv18624 Modified Files: SkinColorGMMTrain.cc Log Message: Fixed a problem while loading a model. Index: SkinColorGMMTrain.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Probes/src/SkinColorGMMTrain.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** SkinColorGMMTrain.cc 1 Jul 2005 17:05:05 -0000 1.2 --- SkinColorGMMTrain.cc 1 Jul 2005 17:53:41 -0000 1.3 *************** *** 586,590 **** int m_outMaskID; - bool m_hasSkinGMM; bool m_hasInit; bool m_adding; --- 586,589 ---- *************** *** 669,672 **** --- 668,673 ---- cSelection->m_skinGMM = myModel; + cSelection->m_hasInit = true; + infile.close(); } |
|
From: Dominic L. <ma...@us...> - 2005-07-01 17:31:20
|
Update of /cvsroot/robotflow/RobotFlow/Vision/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv7298 Modified Files: Makefile.am SkinColorGMMSegm.cc Log Message: fixed memory leak when no skin detected Index: SkinColorGMMSegm.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/src/SkinColorGMMSegm.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** SkinColorGMMSegm.cc 27 Jun 2005 21:12:34 -0000 1.2 --- SkinColorGMMSegm.cc 1 Jul 2005 17:31:08 -0000 1.3 *************** *** 2,8 **** #define _SKINCOLORGMMSEGM_CC_ ! #include "BufferedNode.h" #include "Image.h" #include "gmm.h" #include <stdlib.h> #include <sys/timeb.h> --- 2,10 ---- #define _SKINCOLORGMMSEGM_CC_ ! #include "Vector.h" ! #include "misc.h" #include "Image.h" #include "gmm.h" + #include "BufferedNode.h" #include <stdlib.h> #include <sys/timeb.h> *************** *** 155,158 **** --- 157,161 ---- else { (*outputs[m_imageOutID].buffer)[count] = ObjectRef(nilObject); + segmented->destroy(); } Index: Makefile.am =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/src/Makefile.am,v retrieving revision 1.36 retrieving revision 1.37 diff -C2 -d -r1.36 -r1.37 *** Makefile.am 23 Jun 2005 20:50:08 -0000 1.36 --- Makefile.am 1 Jul 2005 17:31:07 -0000 1.37 *************** *** 98,102 **** libVision_la_LDFLAGS = -release $(LT_RELEASE) $(PIXBUF_LIBS) $(GNOME_LIB) $(JPEG_LIB) $(OPENCV_LIBS) ! INCLUDES = -I../include $(OVERFLOW_INCLUDE) $(PIXBUF_INCLUDE) $(JPEG_INCLUDE) $(OPENCV_INCLUDES) -I$(OVERFLOW_DATA)/HMM install-data-local: --- 98,102 ---- libVision_la_LDFLAGS = -release $(LT_RELEASE) $(PIXBUF_LIBS) $(GNOME_LIB) $(JPEG_LIB) $(OPENCV_LIBS) ! INCLUDES = -I../include $(OVERFLOW_INCLUDE) -I$(OVERFLOW_DATA)/HMM $(PIXBUF_INCLUDE) $(JPEG_INCLUDE) $(OPENCV_INCLUDES) install-data-local: |
|
From: Pierre M. <sid...@us...> - 2005-07-01 17:05:21
|
Update of /cvsroot/robotflow/RobotFlow/Probes/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv24999 Modified Files: SkinColorGMMTrain.cc Log Message: Removed non skin modelling, added load and show button, added parameters for multiple thresholds testing. Index: SkinColorGMMTrain.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Probes/src/SkinColorGMMTrain.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** SkinColorGMMTrain.cc 23 Jun 2005 20:57:32 -0000 1.1 --- SkinColorGMMTrain.cc 1 Jul 2005 17:05:05 -0000 1.2 *************** *** 63,67 **** * @parameter_type string * @parameter_value ./ColorGMM ! * @parameter_description File basename for the models to be saved. * * @parameter_name NUM_SKIN_GAUSSIANS --- 63,67 ---- * @parameter_type string * @parameter_value ./ColorGMM ! * @parameter_description Filename for the models to be saved. * * @parameter_name NUM_SKIN_GAUSSIANS *************** *** 70,87 **** * @parameter_description Number of gaussians to use in the skin mixture model. * ! * @parameter_name NUM_NONSKIN_GAUSSIANS ! * @parameter_type int ! * @parameter_value 3 ! * @parameter_description Number of gaussians to use in the non skin mixture model. ! * ! * @parameter_name SKIN_LOG_LIKELIHOOD_THRESHOLD * @parameter_type float ! * @parameter_value 10.0 ! * @parameter_description Log likelihood threshold to consider a pixel as skin. * ! * @parameter_name NON_SKIN_LOG_LIKELIHOOD_THRESHOLD * @parameter_type float ! * @parameter_value 10.0 ! * @parameter_description Log likelihood threshold to consider a pixel as non skin. * * @input_name IN_IMAGE --- 70,82 ---- * @parameter_description Number of gaussians to use in the skin mixture model. * ! * @parameter_name MEAN_SKIN_LOG_LIKELIHOOD_THRESHOLD * @parameter_type float ! * @parameter_value 5.3 ! * @parameter_description Mean log likelihood threshold to consider a pixel as skin. * ! * @parameter_name SKIN_LOG_LIKELIHOOD_THRESHOLD_INC * @parameter_type float ! * @parameter_value 0.1 ! * @parameter_description Increment for the mean log likelihood threshold. This is to test multiple thresholds. * * @input_name IN_IMAGE *************** *** 95,115 **** END*/ - static const char k_SCGMMTP_modelColorName[2][5] = {"blue", "red"}; - class SkinColorGMMTrain : public BufferedNode { friend void on_save_button_clicked (GtkButton *button, SkinColorGMMTrain *cSelection); friend void on_add_button_clicked (GtkButton *button, SkinColorGMMTrain *cSelection); friend void on_init_button_clicked (GtkButton *button, SkinColorGMMTrain *cSelection); friend void on_adapt_button_clicked (GtkButton *button, SkinColorGMMTrain *cSelection); - friend void on_model_button_clicked (GtkButton *button, SkinColorGMMTrain *cSelection); friend gboolean pixbuf_event_function (GnomeCanvasItem *canvasitem, GdkEvent *event, SkinColorGMMTrain *cSelection); public: SkinColorGMMTrain(string nodeName, ParameterSet params) ! : BufferedNode(nodeName, params), m_hasInit(false), m_hasSkinGMM(false), ! m_hasNonSkinGMM(false), m_saving(false), m_adding(false), ! m_initializing(false), m_adapting(false), m_useSkinModel(true), m_skip(0), ! m_pixbuf_item(NULL), m_last_count(-1) { try { --- 90,110 ---- END*/ class SkinColorGMMTrain : public BufferedNode { + friend void on_load_button_clicked (GtkButton *button, SkinColorGMMTrain *cSelection); + friend void on_load_button_clicked_ok (GtkButton *button, SkinColorGMMTrain *cSelection); + friend void on_load_button_clicked_cancel (GtkButton *button, SkinColorGMMTrain *cSelection); friend void on_save_button_clicked (GtkButton *button, SkinColorGMMTrain *cSelection); + friend void on_show_button_clicked (GtkButton *button, SkinColorGMMTrain *cSelection); friend void on_add_button_clicked (GtkButton *button, SkinColorGMMTrain *cSelection); friend void on_init_button_clicked (GtkButton *button, SkinColorGMMTrain *cSelection); friend void on_adapt_button_clicked (GtkButton *button, SkinColorGMMTrain *cSelection); friend gboolean pixbuf_event_function (GnomeCanvasItem *canvasitem, GdkEvent *event, SkinColorGMMTrain *cSelection); public: SkinColorGMMTrain(string nodeName, ParameterSet params) ! : BufferedNode(nodeName, params), m_hasInit(false), ! m_saving(false), m_adding(false), m_initializing(false), m_adapting(false), m_show(true), ! m_skip(0), m_pixbuf_item(NULL), m_last_count(-1) { try { *************** *** 125,131 **** m_outModelName = object_cast<string>(parameters.get("OUT_MODEL_FILENAME")); m_numSkinGaussians = dereference_cast<int>(parameters.get("NUM_SKIN_GAUSSIANS")); ! m_numNonSkinGaussians = dereference_cast<int>(parameters.get("NUM_NONSKIN_GAUSSIANS")); ! m_skinProbThresh = dereference_cast<float>(parameters.get("SKIN_LOG_LIKELIHOOD_THRESHOLD")); ! m_nonSkinProbThresh = dereference_cast<float>(parameters.get("NON_SKIN_LOG_LIKELIHOOD_THRESHOLD")); m_numPixels = m_width*m_height; --- 120,125 ---- m_outModelName = object_cast<string>(parameters.get("OUT_MODEL_FILENAME")); m_numSkinGaussians = dereference_cast<int>(parameters.get("NUM_SKIN_GAUSSIANS")); ! m_meanSkinProbThresh = dereference_cast<float>(parameters.get("MEAN_SKIN_LOG_LIKELIHOOD_THRESHOLD")); ! m_skinProbThreshInc = dereference_cast<float>(parameters.get("SKIN_LOG_LIKELIHOOD_THRESHOLD_INC")); m_numPixels = m_width*m_height; *************** *** 147,151 **** m_numDims = 2; m_skinGMM = RCPtr<GMM>(new GMM(m_numSkinGaussians, m_numDims, NewDiagonalCovariance)); - m_nonSkinGMM = RCPtr<GMM>(new GMM(m_numNonSkinGaussians, m_numDims, NewDiagonalCovariance)); m_tmpData = Vector<float>::alloc(2); } --- 141,144 ---- *************** *** 220,223 **** --- 213,224 ---- gtk_widget_show (hbox1); gtk_box_pack_start (GTK_BOX (vbox2), hbox1, TRUE, TRUE, 0); + + //LOAD + load_button = gtk_button_new_with_label (_("Load")); + gtk_widget_ref (load_button); + gtk_object_set_data_full (GTK_OBJECT (window1), "load_button", load_button, + (GtkDestroyNotify) gtk_widget_unref); + gtk_widget_show (load_button); + gtk_box_pack_start (GTK_BOX (hbox1), load_button, FALSE, FALSE, 0); //SAVE *************** *** 229,233 **** gtk_box_pack_start (GTK_BOX (hbox1), save_button, FALSE, FALSE, 0); ! //ADD BUTTON add_button = gtk_button_new_with_label (_("Add Pixels")); gtk_widget_ref (add_button); --- 230,242 ---- gtk_box_pack_start (GTK_BOX (hbox1), save_button, FALSE, FALSE, 0); ! //SHOW ! show_button = gtk_button_new_with_label (_("Show")); ! gtk_widget_ref (show_button); ! gtk_object_set_data_full (GTK_OBJECT (window1), "show_button", show_button, ! (GtkDestroyNotify) gtk_widget_unref); ! gtk_widget_show (show_button); ! gtk_box_pack_start (GTK_BOX (hbox1), show_button, FALSE, FALSE, 0); ! ! //ADD add_button = gtk_button_new_with_label (_("Add Pixels")); gtk_widget_ref (add_button); *************** *** 237,241 **** gtk_box_pack_start (GTK_BOX (hbox1), add_button, FALSE, TRUE, 0); ! //INIT BUTTON init_button = gtk_button_new_with_label (_("Init")); gtk_widget_ref (init_button); --- 246,250 ---- gtk_box_pack_start (GTK_BOX (hbox1), add_button, FALSE, TRUE, 0); ! //INIT init_button = gtk_button_new_with_label (_("Init")); gtk_widget_ref (init_button); *************** *** 245,249 **** gtk_box_pack_start (GTK_BOX (hbox1), init_button, FALSE, TRUE, 0); ! //ADAPT BUTTON adapt_button = gtk_button_new_with_label (_("Adapt")); gtk_widget_ref (adapt_button); --- 254,258 ---- gtk_box_pack_start (GTK_BOX (hbox1), init_button, FALSE, TRUE, 0); ! //ADAPT adapt_button = gtk_button_new_with_label (_("Adapt")); gtk_widget_ref (adapt_button); *************** *** 252,263 **** gtk_widget_show (adapt_button); gtk_box_pack_start (GTK_BOX (hbox1), adapt_button, FALSE, TRUE, 0); - - //MODEL BUTTON - model_button = gtk_button_new_with_label (_("Switch Model")); - gtk_widget_ref (model_button); - gtk_object_set_data_full (GTK_OBJECT (window1), "model_button", model_button, - (GtkDestroyNotify) gtk_widget_unref); - gtk_widget_show (model_button); - gtk_box_pack_start (GTK_BOX (hbox1), model_button, FALSE, TRUE, 0); //creating pixbuf item --- 261,264 ---- *************** *** 275,281 **** --- 276,288 ---- //connecting signals + gtk_signal_connect (GTK_OBJECT (load_button), "clicked", + GTK_SIGNAL_FUNC (on_load_button_clicked), + this); gtk_signal_connect (GTK_OBJECT (save_button), "clicked", GTK_SIGNAL_FUNC (on_save_button_clicked), this); + gtk_signal_connect (GTK_OBJECT (show_button), "clicked", + GTK_SIGNAL_FUNC (on_show_button_clicked), + this); gtk_signal_connect (GTK_OBJECT (add_button), "clicked", GTK_SIGNAL_FUNC (on_add_button_clicked), *************** *** 287,293 **** GTK_SIGNAL_FUNC (on_adapt_button_clicked), this); - gtk_signal_connect (GTK_OBJECT (model_button), "clicked", - GTK_SIGNAL_FUNC (on_model_button_clicked), - this); gtk_signal_connect(GTK_OBJECT(m_pixbuf_item), "event", GTK_SIGNAL_FUNC(pixbuf_event_function), --- 294,297 ---- *************** *** 341,356 **** } ! memcpy(m_curFrame->imageData, object_cast<Image>(m_imageRef).get_data(), m_numBytesInFrame); ! if (m_adding) { ! cout << "Adding pixels to model " << m_useSkinModel << endl; ! if (m_useSkinModel) { ! AddPixelsToGMM((const unsigned char *)(m_curFrame->imageData), &m_skinData); ! } ! else { ! AddPixelsToGMM((const unsigned char *)(m_curFrame->imageData), &m_nonSkinData); ! } m_adding = false; --- 345,356 ---- } ! if (m_show) { ! memcpy(m_curFrame->imageData, object_cast<Image>(m_imageRef).get_data(), m_numBytesInFrame); ! } if (m_adding) { ! cout << "Adding pixels to color model." << endl; ! AddPixelsToGMM((const unsigned char *)(m_curFrame->imageData), &m_skinData); m_adding = false; *************** *** 358,387 **** if (m_initializing) { ! cout << "Initializing model " << m_useSkinModel << endl; ! if (m_useSkinModel) { ! InitGMM(m_skinGMM.get(), &m_skinData); ! m_hasSkinGMM = true; ! } ! else { ! InitGMM(m_nonSkinGMM.get(), &m_nonSkinData); ! m_hasNonSkinGMM = true; ! } - if (m_hasSkinGMM && m_hasNonSkinGMM) { - m_hasInit = true; - } m_initializing = false; } if (m_adapting) { ! cout << "Adapting model " << m_useSkinModel << endl; ! if (m_useSkinModel) { ! AdaptGMM(m_skinGMM.get(), &m_skinData); ! } ! else { ! AdaptGMM(m_nonSkinGMM.get(), &m_nonSkinData); ! } m_adapting = false; --- 358,373 ---- if (m_initializing) { ! cout << "Initializing color model." << endl; ! InitGMM(m_skinGMM.get(), &m_skinData); ! m_hasInit = true; m_initializing = false; } if (m_adapting) { ! cout << "Adapting color model." << endl; ! AdaptGMM(m_skinGMM.get(), &m_skinData); m_adapting = false; *************** *** 389,393 **** if (m_saving) { ! cout << "Saving models." << endl; SaveModels(); --- 375,379 ---- if (m_saving) { ! cout << "Saving model." << endl; SaveModels(); *************** *** 402,456 **** CvRect roi; - m_skinProbThresh = 2.0f; - SegmentSkin((const unsigned char *)(m_curFrame->imageData), (unsigned char *)(m_tmpFrame->imageData)); - - cvSmooth(m_tmpFrame, m_maskFrame, CV_MEDIAN, 3, 0); roi.x = 0; roi.y = 0; roi.width = m_width/3; roi.height = m_height/2; - cvSetImageROI(m_outFrame,roi); - cvResize(m_maskFrame, m_outFrame, CV_INTER_CUBIC); - - m_skinProbThresh = 5.0f; - SegmentSkin((const unsigned char *)(m_curFrame->imageData), (unsigned char *)(m_tmpFrame->imageData)); - - cvSmooth(m_tmpFrame, m_maskFrame, CV_MEDIAN, 3, 0); - roi.x += roi.width; - cvSetImageROI(m_outFrame,roi); - cvResize(m_maskFrame, m_outFrame, CV_INTER_CUBIC); - - m_skinProbThresh = 5.1f; - SegmentSkin((const unsigned char *)(m_curFrame->imageData), (unsigned char *)(m_tmpFrame->imageData)); - - cvSmooth(m_tmpFrame, m_maskFrame, CV_MEDIAN, 3, 0); - roi.x += roi.width; - cvSetImageROI(m_outFrame,roi); - cvResize(m_maskFrame, m_outFrame, CV_INTER_CUBIC); - - m_skinProbThresh = 5.2f; - SegmentSkin((const unsigned char *)(m_curFrame->imageData), (unsigned char *)(m_tmpFrame->imageData)); ! cvSmooth(m_tmpFrame, m_maskFrame, CV_MEDIAN, 3, 0); ! roi.x = 0; ! roi.y += roi.height; ! cvSetImageROI(m_outFrame,roi); ! cvResize(m_maskFrame, m_outFrame, CV_INTER_CUBIC); ! ! m_skinProbThresh = 5.3f; ! SegmentSkin((const unsigned char *)(m_curFrame->imageData), (unsigned char *)(m_tmpFrame->imageData)); ! ! cvSmooth(m_tmpFrame, m_maskFrame, CV_MEDIAN, 3, 0); ! roi.x += roi.width; ! cvSetImageROI(m_outFrame,roi); ! cvResize(m_maskFrame, m_outFrame, CV_INTER_CUBIC); ! ! m_skinProbThresh = 5.4f; ! SegmentSkin((const unsigned char *)(m_curFrame->imageData), (unsigned char *)(m_tmpFrame->imageData)); ! cvSmooth(m_tmpFrame, m_maskFrame, CV_MEDIAN, 3, 0); ! roi.x += roi.width; ! cvSetImageROI(m_outFrame,roi); ! cvResize(m_maskFrame, m_outFrame, CV_INTER_CUBIC); roi.x = 0; --- 388,415 ---- CvRect roi; roi.x = 0; roi.y = 0; roi.width = m_width/3; roi.height = m_height/2; ! float curThresh = m_meanSkinProbThresh - 2*m_skinProbThreshInc; ! for (int m=0; m<6; m++) { ! SegmentSkin( ! (const unsigned char *)(m_curFrame->imageData), ! (unsigned char *)(m_tmpFrame->imageData), ! curThresh); ! cvSmooth(m_tmpFrame, m_maskFrame, CV_MEDIAN, 3, 0); ! cvSetImageROI(m_outFrame,roi); ! cvResize(m_maskFrame, m_outFrame, CV_INTER_LINEAR); ! ! roi.x += roi.width; ! if (m == 2) { ! roi.x = 0; ! roi.y += roi.height; ! } ! ! curThresh += m_skinProbThreshInc; ! } roi.x = 0; *************** *** 474,478 **** void SegmentSkin(const unsigned char *i_srcImg, ! unsigned char *o_skinMask) { unsigned char val; --- 433,437 ---- void SegmentSkin(const unsigned char *i_srcImg, ! unsigned char *o_skinMask, float i_thresh) { unsigned char val; *************** *** 486,498 **** float skinlogLikelihood = -m_skinGMM->score(&((*m_tmpData)[0])).score; - //float nonSkinlogLikelihood = -m_nonSkinGMM->score(&((*m_tmpData)[0])).score; - - //cout << "nonSkinlogLikelihood=" << nonSkinlogLikelihood << endl; - //cout << "skinlogLikelihood=" << skinlogLikelihood << " nonSkinlogLikelihood=" << nonSkinlogLikelihood << endl; - //float logLikelihood = skinlogLikelihood; ! if (/*skinlogLikelihood > m_skinProbThresh && ! nonSkinlogLikelihood < m_nonSkinProbThresh*/ ! skinlogLikelihood > m_skinProbThresh) { *p_maskPtr++ = 255; } --- 445,450 ---- float skinlogLikelihood = -m_skinGMM->score(&((*m_tmpData)[0])).score; ! if (skinlogLikelihood > i_thresh) { *p_maskPtr++ = 255; } *************** *** 603,618 **** skinModelFile.close(); - - string nonSkinFilename = m_outModelName + "NonSkin.txt"; - ofstream nonSkinModelFile(nonSkinFilename.c_str(), ios::out); - - if (!nonSkinModelFile) { - throw new GeneralException (string("SkinColorGMMTrain::WriteModels : unable to open file: ") + - nonSkinFilename,__FILE__,__LINE__); - } - - m_skinGMM->printOn(nonSkinModelFile); - - nonSkinModelFile.close(); } catch (BaseException *e) { --- 555,558 ---- *************** *** 647,651 **** bool m_hasSkinGMM; - bool m_hasNonSkinGMM; bool m_hasInit; bool m_adding; --- 587,590 ---- *************** *** 653,657 **** bool m_adapting; bool m_saving; ! bool m_useSkinModel; int m_skip; --- 592,596 ---- bool m_adapting; bool m_saving; ! bool m_show; int m_skip; *************** *** 672,680 **** GtkWidget *canvas1; GtkWidget *hbox1; GtkWidget *save_button; GtkWidget *add_button; GtkWidget *init_button; GtkWidget *adapt_button; ! GtkWidget *model_button; GnomeCanvasItem *m_pixbuf_item; --- 611,621 ---- GtkWidget *canvas1; GtkWidget *hbox1; + GtkWidget *load_button; GtkWidget *save_button; + GtkWidget *show_button; GtkWidget *add_button; GtkWidget *init_button; GtkWidget *adapt_button; ! GtkWidget *m_file_selection; GnomeCanvasItem *m_pixbuf_item; *************** *** 691,705 **** int m_numSkinGaussians; - int m_numNonSkinGaussians; int m_numDims; ! float m_skinProbThresh; ! float m_nonSkinProbThresh; RCPtr<GMM> m_skinGMM; - RCPtr<GMM> m_nonSkinGMM; Vector<float> *m_tmpData; Vector<Vector<float> > m_skinData; - Vector<Vector<float> > m_nonSkinData; }; void on_save_button_clicked (GtkButton *button, --- 632,690 ---- int m_numSkinGaussians; int m_numDims; ! float m_meanSkinProbThresh; ! float m_skinProbThreshInc; RCPtr<GMM> m_skinGMM; Vector<float> *m_tmpData; Vector<Vector<float> > m_skinData; }; + void on_load_button_clicked (GtkButton *button, SkinColorGMMTrain *cSelection) + { + cSelection->m_file_selection = gtk_file_selection_new ("Select GMM Model"); + + gtk_file_selection_hide_fileop_buttons(GTK_FILE_SELECTION(cSelection->m_file_selection)); + + //callbacks + gtk_signal_connect(GTK_OBJECT (GTK_FILE_SELECTION(cSelection->m_file_selection)->ok_button), + "clicked", GTK_SIGNAL_FUNC(on_load_button_clicked_ok), cSelection); + + gtk_signal_connect(GTK_OBJECT (GTK_FILE_SELECTION(cSelection->m_file_selection)->cancel_button), + "clicked", GTK_SIGNAL_FUNC(on_load_button_clicked_cancel), cSelection); + + gtk_widget_show(GTK_WIDGET(cSelection->m_file_selection)); + } + + void on_load_button_clicked_ok (GtkButton *button, SkinColorGMMTrain *cSelection) + { + const char *filename = gtk_file_selection_get_filename (GTK_FILE_SELECTION(cSelection->m_file_selection)); + + try { + ifstream infile(filename); + + ObjectRef myModel; + + infile >> myModel; + + cSelection->m_skinGMM = myModel; + + infile.close(); + } + catch (BaseException *e) { + cerr<<"Unable to load model file : " << filename << endl; + delete e; + } + + cout << "Model file loaded : " << filename << endl; + + gtk_widget_destroy(cSelection->m_file_selection); + } + + void on_load_button_clicked_cancel (GtkButton *button, SkinColorGMMTrain *cSelection) + { + //do nothing + gtk_widget_destroy(cSelection->m_file_selection); + } + void on_save_button_clicked (GtkButton *button, *************** *** 709,712 **** --- 694,703 ---- } + void on_show_button_clicked (GtkButton *button, + SkinColorGMMTrain *cSelection) + { + cSelection->m_show = !cSelection->m_show; + } + void on_add_button_clicked (GtkButton *button, SkinColorGMMTrain *cSelection) *************** *** 727,736 **** } - void on_model_button_clicked (GtkButton *button, - SkinColorGMMTrain *cSelection) - { - cSelection->m_useSkinModel = !cSelection->m_useSkinModel; - } - gboolean pixbuf_event_function( GnomeCanvasItem *canvasitem, --- 718,721 ---- *************** *** 754,763 **** if (!cSelection->m_imageRef->isNil()) { int colorNameIdx; - if (cSelection->m_useSkinModel) { - colorNameIdx = 0; - } - else { - colorNameIdx = 1; - } item = gnome_canvas_item_new ( --- 739,742 ---- *************** *** 769,773 **** "y2",item_y, "outline_color", ! k_SCGMMTP_modelColorName[colorNameIdx], NULL); --- 748,752 ---- "y2",item_y, "outline_color", ! "blue", NULL); |
|
From: MagellanPro <mag...@us...> - 2005-07-01 15:03:25
|
Update of /cvsroot/robotflow/RobotFlow/Devices/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv22308 Modified Files: SNCRZ30RS232.cc Log Message: ending session properly Index: SNCRZ30RS232.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Devices/src/SNCRZ30RS232.cc,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** SNCRZ30RS232.cc 30 Jun 2005 19:16:43 -0000 1.4 --- SNCRZ30RS232.cc 1 Jul 2005 15:03:16 -0000 1.5 *************** *** 854,857 **** --- 854,858 ---- cerr << "SNCRZ30RS232 (done!)" << endl; + End_session(fd); } } |
|
From: MagellanPro <mag...@us...> - 2005-07-01 01:52:46
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv14759 Modified Files: RangeFashion.cc Log Message: Tested version (first iteration) Index: RangeFashion.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/RangeFashion.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** RangeFashion.cc 30 Jun 2005 21:13:21 -0000 1.1 --- RangeFashion.cc 1 Jul 2005 01:52:34 -0000 1.2 *************** *** 95,103 **** int i=0; for(iterIR = irReading.begin(); iterIR != irReading.end(); ++iterIR){ ! if(*iterIR <= 100) // Max value IR (*vectPercept)[i]=*iterIR; ! ++i; } } // Priority #2 --- 95,110 ---- int i=0; for(iterIR = irReading.begin(); iterIR != irReading.end(); ++iterIR){ ! if(*iterIR <= 1000) // Max value IR (*vectPercept)[i]=*iterIR; ! ++i; } } + + std::cout << "IR -> "; + Vector<int>::iterator iterA; + for(iterA = vectPercept->begin(); iterA != vectPercept->end(); ++iterA){ + std::cout << *iterA << " "; + } + std::cout << std::endl; // Priority #2 *************** *** 157,172 **** int i=0; for(iterSonar = sonarReading.begin(); iterSonar != sonarReading.end(); ++iterSonar) { ! if((*iterSonar = -1) && (*iterSonar<10000)) // Max value Sonar (*vectPercept)[i]=*iterSonar; ++i; } } ! // 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; ! } } --- 164,185 ---- int i=0; for(iterSonar = sonarReading.begin(); iterSonar != sonarReading.end(); ++iterSonar) { ! if(((*vectPercept)[i] == -1) && (*iterSonar<10000)) // Max value Sonar (*vectPercept)[i]=*iterSonar; ++i; } } ! ! 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; ! }*/ } |
|
From: Fernyqc <fe...@us...> - 2005-06-30 21:15:36
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv28345/Behaviors/src Modified Files: Makefile.am Log Message: RangeFashion.cc is include in that Makefile.am Index: Makefile.am =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/Makefile.am,v retrieving revision 1.15 retrieving revision 1.16 diff -C2 -d -r1.15 -r1.16 *** Makefile.am 18 Jun 2005 15:08:20 -0000 1.15 --- Makefile.am 30 Jun 2005 21:15:28 -0000 1.16 *************** *** 32,36 **** Goto.cc \ GotoPat.cc \ ! FollowWall.cc libBehaviors_la_LDFLAGS = -release $(LT_RELEASE) --- 32,37 ---- Goto.cc \ GotoPat.cc \ ! FollowWall.cc \ ! RangeFashion.cc libBehaviors_la_LDFLAGS = -release $(LT_RELEASE) |
|
From: Fernyqc <fe...@us...> - 2005-06-30 21:13:29
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv27141 Added Files: RangeFashion.cc Log Message: First version of the file --- NEW FILE: RangeFashion.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 _RANGE_FASHION_CC_ #define _RANGE_FASHION_CC_ #include "Behavior.h" #include "Vector.h" using namespace std; using namespace FD; namespace RobotFlow { class RangeFashion; DECLARE_NODE(RangeFashion) REGISTER_BEHAVIOR(RangeFashion) /*Node * @name RangeFashion * @category RobotFlow:Behaviors * @description Fusion range device in a unique percept * * @input_name ACTIVATED * @input_type bool * @input_description Behavior activation * * @input_name LASERS * @input_type Vector<int> * @input_description All lasers reading. * * @input_name SONARS * @input_type Vector<int> * @input_description All sonars reading. * * @input_name IRS * @input_type Vector<int> * @input_description All IRs reading. * * @output_name PERCEPT * @output_type Vector<int> * @output_description Percept after fusion * * @output_name EXPLOITATION * @output_description Behavior exploitation * * @parameter_name TYPE_FASHION * @parameter_description Determine of to make the sensor fashion * @parameter_type int * @parameter_value 0 * END*/ class RangeFashion : public Behavior { //inputs int laserID; int sonarID; int irID; //outputs int perceptID; //parameters int m_typeFashion; void priorityBaseFashion(Vector<int> *vectPercept, ObjectRef &laserValue, ObjectRef &sonarValue, ObjectRef &irValue, int count){ // Init vectPercept to -1 vectPercept->assign(12, -1); // Priority #1 // IRs if(!irValue->isNil()){ Vector<int> &irReading = object_cast<Vector<int> >(getInput(irID,count)); // Copy IR values Vector<int>::iterator iterIR; int i=0; for(iterIR = irReading.begin(); iterIR != irReading.end(); ++iterIR){ if(*iterIR <= 100) // Max value IR (*vectPercept)[i]=*iterIR; ++i; } } // Priority #2 // Lasers if(!laserValue->isNil()){ Vector<int> &laserReading = object_cast<Vector<int> >(getInput(laserID,count)); int ind_vectPercept = 3; // For each position in the range of the laser for(int i=0; i<7; ++i){ // Determine the range for the position int begin = 60 * i - 10; int end = 60 * i + 10; if (i==0) begin = 0; else if (i==6) end = 360; // Get the 3 mins of the range int min1=10000, min2=10000, min3=10000; for(int j=begin; j<end; ++j){ if(laserReading[j] <= min1){ min3 = min2; min2 = min1; min1 = laserReading[j]; } else if(laserReading[j] <= min2){ min3 = min2; min2 = laserReading[j]; } else if(laserReading[j] <= min3){ min3 = laserReading[j]; } } // Evaluate the mean of the min int x_min = (min1+min2+min3)/3; // Save the value if(((*vectPercept)[ind_vectPercept] == -1) && (x_min < 10000)) (*vectPercept)[ind_vectPercept] = x_min; // Next position --ind_vectPercept; if(ind_vectPercept<0) ind_vectPercept = 11; } } // Priority #3 // Sonars if(!sonarValue->isNil()){ Vector<int> &sonarReading = object_cast<Vector<int> >(getInput(sonarID,count)); // Copy Sonar values Vector<int>::iterator iterSonar; int i=0; for(iterSonar = sonarReading.begin(); iterSonar != sonarReading.end(); ++iterSonar) { if((*iterSonar = -1) && (*iterSonar<10000)) // Max value Sonar (*vectPercept)[i]=*iterSonar; ++i; } } // 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; } } public: RangeFashion(string nodeName, ParameterSet params) : Behavior(nodeName, params, "RangeFashion") { //inputs laserID = addInput("LASERS"); sonarID = addInput("SONARS"); irID = addInput("IRS"); //outputs perceptID = addOutput("PERCEPT"); // parameters m_typeFashion = dereference_cast<int>(parameters.get("TYPE_FASHION")); } void calculate_behavior(int output_id, int count, Buffer &out) { ObjectRef laserValue = getInput(laserID,count); ObjectRef sonarValue = getInput(sonarID,count); ObjectRef irValue = getInput(irID,count); Vector<int> *vectPercept = Vector<int>::alloc(12); ObjectRef percept = nilObject; switch (m_typeFashion) { case 0: priorityBaseFashion(vectPercept, laserValue, sonarValue, irValue, count); percept = ObjectRef(vectPercept); break; } out[count] = percept; }//calculate_behavior }; }//namespace RobotFlow #endif |
|
From: Dominic L. <ma...@us...> - 2005-06-30 20:43:02
|
Update of /cvsroot/robotflow/RobotFlow/Vision/n-files In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv9429 Modified Files: ChallengeVision.n Log Message: now using ABSOLUTE position Index: ChallengeVision.n =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/n-files/ChallengeVision.n,v retrieving revision 1.5 retrieving revision 1.6 diff -C2 -d -r1.5 -r1.6 *** ChallengeVision.n 28 Jun 2005 14:24:22 -0000 1.5 --- ChallengeVision.n 30 Jun 2005 20:42:52 -0000 1.6 *************** *** 91,115 **** <Parameter name="HORIZONTAL_TOLERANCE" type="float" value="0.8" description="No description available"/> </Node> ! <Node name="node_CAMERA_CTRL_1" type="CAMERA_CTRL" x="1820.000000" y="117.000000"/> <Node name="node_Subsumption_1" type="Subsumption" x="1653.000000" y="117.000000"/> <Node name="node_Subsumption_2" type="Subsumption" x="2174.000000" y="-90.000000"/> ! <Node name="node_IF_1" type="IF" x="2113.000000" y="-5.000000"> <Parameter name="PULL_ANYWAY" type="bool" value="" description="If true, the IF statement pulls also on the branch not taken"/> </Node> ! <Node name="node_isNil_1" type="isNil" x="1768.000000" y="-19.000000"/> ! <Node name="node_NilObject_2" type="NilObject" x="1827.000000" y="10.000000"/> ! <Node name="node_IF_2" type="IF" x="1903.000000" y="357.000000"> <Parameter name="PULL_ANYWAY" type="bool" value="" description="If true, the IF statement pulls also on the branch not taken"/> </Node> ! <Node name="node_isNil_2" type="isNil" x="1777.000000" y="341.000000"/> ! <Node name="node_Constant_10" type="Constant" x="1679.000000" y="356.000000"> <Parameter name="VALUE" type="subnet_param" value="DEFAULT_ABS_PAN_COMMAND" description="The value"/> </Node> ! <Node name="node_NilObject_3" type="NilObject" x="1604.000000" y="371.000000"/> ! <Node name="node_Constant_12" type="Constant" x="1908.000000" y="-6.000000"> <Parameter name="VALUE" type="subnet_param" value="DEFAULT_ABS_TILT_COMMAND" description="The value"/> </Node> ! <Node name="node_Add_1" type="Add" x="2052.000000" y="196.000000"/> ! <Node name="node_Max_1" type="Max" x="2156.000000" y="293.000000"/> <Node name="node_Constant_1" type="Constant" x="1917.000000" y="300.000000"> <Parameter name="VALUE" type="int" value="0" description="The value"/> --- 91,112 ---- <Parameter name="HORIZONTAL_TOLERANCE" type="float" value="0.8" description="No description available"/> </Node> ! <Node name="node_CAMERA_CTRL_1" type="CAMERA_CTRL" x="1908.000000" y="134.000000"/> <Node name="node_Subsumption_1" type="Subsumption" x="1653.000000" y="117.000000"/> <Node name="node_Subsumption_2" type="Subsumption" x="2174.000000" y="-90.000000"/> ! <Node name="node_IF_1" type="IF" x="2307.000000" y="-5.000000"> <Parameter name="PULL_ANYWAY" type="bool" value="" description="If true, the IF statement pulls also on the branch not taken"/> </Node> ! <Node name="node_isNil_1" type="isNil" x="1962.000000" y="-19.000000"/> ! <Node name="node_IF_2" type="IF" x="2346.000000" y="356.000000"> <Parameter name="PULL_ANYWAY" type="bool" value="" description="If true, the IF statement pulls also on the branch not taken"/> </Node> ! <Node name="node_isNil_2" type="isNil" x="2220.000000" y="340.000000"/> ! <Node name="node_Constant_10" type="Constant" x="2122.000000" y="355.000000"> <Parameter name="VALUE" type="subnet_param" value="DEFAULT_ABS_PAN_COMMAND" description="The value"/> </Node> ! <Node name="node_Constant_12" type="Constant" x="2102.000000" y="-6.000000"> <Parameter name="VALUE" type="subnet_param" value="DEFAULT_ABS_TILT_COMMAND" description="The value"/> </Node> ! <Node name="node_Max_1" type="Max" x="2347.000000" y="157.000000"/> <Node name="node_Constant_1" type="Constant" x="1917.000000" y="300.000000"> <Parameter name="VALUE" type="int" value="0" description="The value"/> *************** *** 129,133 **** <Node name="node_Subsumption_3" type="Subsumption" x="1638.000000" y="-173.000000"/> <Node name="node_NOP_2" type="NOP" x="2105.000000" y="-173.000000"/> - <Node name="node_NOP_3" type="NOP" x="1787.000000" y="205.000000"/> <Node name="node_NOP_4" type="NOP" x="1.000000" y="82.000000"/> <Node name="node_NOP_5" type="NOP" x="-2.000000" y="202.000000"/> --- 126,129 ---- *************** *** 147,150 **** --- 143,149 ---- <Parameter name="PULL_ANYWAY" type="bool" value="false" description="Pull on INPUT2 even if INPUT1 is false"/> </Node> + <Node name="node_NOP_12" type="NOP" x="1517.000000" y="188.000000"/> + <Node name="node_NOP_13" type="NOP" x="1513.000000" y="239.000000"/> + <Node name="node_NOP_14" type="NOP" x="1513.000000" y="286.000000"/> <Link from="node_NOP_1" output="OUTPUT" to="node_CvFaceDetection_1" input="IN_IMAGE">17 122 117 122 117 167 438 167.5 </Link> <Link from="node_NOP_1" output="OUTPUT" to="node_MultiIntegralCuesPFTracker_1" input="IMAGE_IN">17 122 116 122 116 -82 839 -82.5 </Link> *************** *** 163,175 **** <Link from="node_MultiIntegralCuesPFTracker_1" output="IMAGE_OUT" to="node_Subsumption_2" input="HIGH_PRIORITY"/> <Link from="node_isNil_1" output="OUTPUT" to="node_IF_1" input="COND"/> - <Link from="node_NilObject_2" output="VALUE" to="node_IF_1" input="ELSE"/> <Link from="node_isNil_2" output="OUTPUT" to="node_IF_2" input="COND"/> <Link from="node_Constant_10" output="VALUE" to="node_IF_2" input="THEN"/> ! <Link from="node_NilObject_3" output="VALUE" to="node_IF_2" input="ELSE"/> ! <Link from="node_Subsumption_1" output="OUTPUT" to="node_isNil_2" input="INPUT">1700.5 117 1728 116 1729 341 1759 341 </Link> ! <Link from="node_Subsumption_1" output="OUTPUT" to="node_isNil_1" input="INPUT">1700.5 117 1728 116 1727 -18 1750 -19 </Link> <Link from="node_Constant_12" output="VALUE" to="node_IF_1" input="THEN"/> <Link from="node_MultiIntegralCuesPFTracker_2" output="ROI_FOR_DETECTION" to="node_CvFaceDetection_1" input="IN_TRACKED_ROI">1294 169.5 1344 171 1344 238 409 237 409 198 438 197.5 </Link> - <Link from="node_Add_1" output="OUTPUT" to="node_Max_1" input="INPUT1">2069.5 196 2075 196 2074 284 2084 285.5 </Link> <Link from="node_Constant_1" output="VALUE" to="node_Max_1" input="INPUT2"/> <Link from="node_NOP_1" output="OUTPUT" to="node_SkinColorGMMSegm_1" input="IN_IMAGE">17 122 116 123 117 188 157 187.5 </Link> --- 162,171 ---- <Link from="node_MultiIntegralCuesPFTracker_1" output="IMAGE_OUT" to="node_Subsumption_2" input="HIGH_PRIORITY"/> <Link from="node_isNil_1" output="OUTPUT" to="node_IF_1" input="COND"/> <Link from="node_isNil_2" output="OUTPUT" to="node_IF_2" input="COND"/> <Link from="node_Constant_10" output="VALUE" to="node_IF_2" input="THEN"/> ! <Link from="node_Subsumption_1" output="OUTPUT" to="node_isNil_2" input="INPUT">1700.5 117 1728 116 1729 341 2202 340 </Link> ! <Link from="node_Subsumption_1" output="OUTPUT" to="node_isNil_1" input="INPUT">1700.5 117 1728 116 1727 -18 1944 -19 </Link> <Link from="node_Constant_12" output="VALUE" to="node_IF_1" input="THEN"/> <Link from="node_MultiIntegralCuesPFTracker_2" output="ROI_FOR_DETECTION" to="node_CvFaceDetection_1" input="IN_TRACKED_ROI">1294 169.5 1344 171 1344 238 409 237 409 198 438 197.5 </Link> <Link from="node_Constant_1" output="VALUE" to="node_Max_1" input="INPUT2"/> <Link from="node_NOP_1" output="OUTPUT" to="node_SkinColorGMMSegm_1" input="IN_IMAGE">17 122 116 123 117 188 157 187.5 </Link> *************** *** 184,189 **** <Link from="node_Subsumption_3" output="OUTPUT" to="node_NOP_2" input="INPUT"/> <Link from="node_MultiIntegralCuesPFTracker_1" output="ROI_FOR_DETECTION" to="node_TextSignDetect_1" input="IN_TRACKED_ROI">1294 -37 1329 -36 1328 17 284 18 284 68 310 67 </Link> - <Link from="node_CAMERA_CTRL_1" output="REL_ZOOM" to="node_Add_1" input="INPUT1"/> - <Link from="node_NOP_3" output="OUTPUT" to="node_Add_1" input="INPUT2"/> <Link from="node_NOP_4" output="OUTPUT" to="node_TextSignDetect_1" input="ACTIVATION"/> <Link from="node_NOP_5" output="OUTPUT" to="node_SkinColorGMMSegm_1" input="ACTIVATION"/> --- 180,183 ---- *************** *** 203,206 **** --- 197,206 ---- <Link from="node_NOP_11" output="OUTPUT" to="node_AND_1" input="INPUT2"/> <Link from="node_AND_1" output="OUTPUT" to="node_READv2_1" input="SIGN_TRACKING_ACTIVATED"/> + <Link from="node_CAMERA_CTRL_1" output="ABS_ZOOM" to="node_Max_1" input="INPUT1"/> + <Link from="node_CAMERA_CTRL_1" output="ABS_TILT" to="node_IF_1" input="ELSE"/> + <Link from="node_CAMERA_CTRL_1" output="ABS_PAN" to="node_IF_2" input="ELSE"/> + <Link from="node_NOP_12" output="OUTPUT" to="node_CAMERA_CTRL_1" input="CURRENT_PAN"/> + <Link from="node_NOP_13" output="OUTPUT" to="node_CAMERA_CTRL_1" input="CURRENT_TILT"/> + <Link from="node_NOP_14" output="OUTPUT" to="node_CAMERA_CTRL_1" input="CURRENT_ZOOM"/> <NetInput name="NNET" node="node_READv2_1" terminal="NNET" object_type="any" description="No description available"/> <NetInput name="DICT" node="node_READv2_1" terminal="DICT" object_type="any" description="No description available"/> *************** *** 211,220 **** <NetOutput name="ORIGINAL_TEXT" node="node_READv2_1" terminal="ORIGINAL_TEXT" object_type="any" description="No description available"/> <NetOutput name="ABS_TILT" node="node_IF_1" terminal="OUTPUT" object_type="any" description="The object from THEN or ELSE depending on COND"/> - <NetOutput name="REL_PAN" node="node_CAMERA_CTRL_1" terminal="REL_PAN" object_type="any" description="The output = The input"/> - <NetOutput name="REL_TILT" node="node_CAMERA_CTRL_1" terminal="REL_TILT" object_type="any" description="The output = The input"/> - <NetOutput name="REL_ZOOM" node="node_CAMERA_CTRL_1" terminal="REL_ZOOM" object_type="any" description="The output = The input"/> <NetOutput name="ABS_PAN" node="node_IF_2" terminal="OUTPUT" object_type="any" description="The object from THEN or ELSE depending on COND"/> <NetOutput name="ABS_ZOOM" node="node_Max_1" terminal="OUTPUT" object_type="any" description="The maximum value between INPUT1 and INPUT2"/> - <NetInput name="CURRENT_ABS_ZOOM" node="node_NOP_3" terminal="INPUT" object_type="any" description="The input"/> <NetInput name="TEXT_DETECT_ACTIVATED" node="node_NOP_4" terminal="INPUT" object_type="any" description="The input"/> <NetInput name="SKIN_DETECT_ACTIVATION" node="node_NOP_5" terminal="INPUT" object_type="any" description="The input"/> --- 211,216 ---- *************** *** 229,232 **** --- 225,231 ---- <NetOutput name="SKIN_DETECTED" node="node_NOT_3" terminal="OUTPUT" object_type="bool" description="Boolean output"/> <NetInput name="TEXT_EXTRACTION_ACTIVATED" node="node_NOP_11" terminal="INPUT" object_type="any" description="The input"/> + <NetInput name="CURRENT_PAN" node="node_NOP_12" terminal="INPUT" object_type="any" description="The input"/> + <NetInput name="CURRENT_TILT" node="node_NOP_13" terminal="INPUT" object_type="any" description="The input"/> + <NetInput name="CURRENT_ZOOM" node="node_NOP_14" terminal="INPUT" object_type="any" description="The input"/> <Note x="0" y="0" visible="0" text="Created with FlowDesigner 0.8.2"/> </Network> *************** *** 272,287 **** <Parameter name="VALUE" type="bool" value="false" description="The value"/> </Node> ! <Node name="node_NOP_2" type="NOP" x="186.000000" y="-301.000000"/> ! <Node name="node_NOP_3" type="NOP" x="187.000000" y="-185.000000"/> ! <Node name="node_ToInt_1" type="ToInt" x="119.000000" y="-300.000000"/> ! <Node name="node_ToInt_2" type="ToInt" x="122.000000" y="-185.000000"/> <Node name="node_ToFloat_1" type="ToFloat" x="-406.000000" y="-272.000000"/> <Node name="node_ToFloat_2" type="ToFloat" x="-414.000000" y="-160.000000"/> <Node name="node_Sub_1" type="Sub" x="-485.000000" y="-272.000000"/> <Node name="node_Sub_2" type="Sub" x="-490.000000" y="-160.000000"/> ! <Node name="node_IF_4" type="IF" x="794.000000" y="119.000000"> <Parameter name="PULL_ANYWAY" type="bool" value="" description="If true, the IF statement pulls also on the branch not taken"/> </Node> ! <Node name="node_Constant_10" type="Constant" x="576.000000" y="74.000000"> <Parameter name="VALUE" type="int" value="0" description="The value"/> </Node> --- 271,286 ---- <Parameter name="VALUE" type="bool" value="false" description="The value"/> </Node> ! <Node name="node_NOP_2" type="NOP" x="765.000000" y="-299.000000"/> ! <Node name="node_NOP_3" type="NOP" x="763.000000" y="-188.000000"/> ! <Node name="node_ToInt_1" type="ToInt" x="523.000000" y="-293.000000"/> ! <Node name="node_ToInt_2" type="ToInt" x="516.000000" y="-177.000000"/> <Node name="node_ToFloat_1" type="ToFloat" x="-406.000000" y="-272.000000"/> <Node name="node_ToFloat_2" type="ToFloat" x="-414.000000" y="-160.000000"/> <Node name="node_Sub_1" type="Sub" x="-485.000000" y="-272.000000"/> <Node name="node_Sub_2" type="Sub" x="-490.000000" y="-160.000000"/> ! <Node name="node_IF_4" type="IF" x="1052.000000" y="114.000000"> <Parameter name="PULL_ANYWAY" type="bool" value="" description="If true, the IF statement pulls also on the branch not taken"/> </Node> ! <Node name="node_Constant_10" type="Constant" x="903.000000" y="115.000000"> <Parameter name="VALUE" type="int" value="0" description="The value"/> </Node> *************** *** 307,311 **** <Parameter name="VALUE" type="int" value="25" description="The value"/> </Node> ! <Node name="node_Constant_13" type="Constant" x="178.000000" y="264.000000"> <Parameter name="VALUE" type="int" value="-50" description="The value"/> </Node> --- 306,310 ---- <Parameter name="VALUE" type="int" value="25" description="The value"/> </Node> ! <Node name="node_Constant_13" type="Constant" x="195.000000" y="267.000000"> <Parameter name="VALUE" type="int" value="-50" description="The value"/> </Node> *************** *** 316,320 **** <Parameter name="VALUE" type="int" value="0" description="The value"/> </Node> ! <Node name="node_Constant_15" type="Constant" x="509.000000" y="134.000000"> <Parameter name="VALUE" type="int" value="50" description="The value"/> </Node> --- 315,319 ---- <Parameter name="VALUE" type="int" value="0" description="The value"/> </Node> ! <Node name="node_Constant_15" type="Constant" x="435.000000" y="64.000000"> <Parameter name="VALUE" type="int" value="50" description="The value"/> </Node> *************** *** 329,334 **** <Node name="node_Min_2" type="Min" x="-519.000000" y="76.000000"/> <Node name="node_Min_3" type="Min" x="-400.000000" y="26.000000"/> ! <Node name="node_NOP_7" type="NOP" x="926.000000" y="119.000000"/> ! <Node name="node_ToInt_3" type="ToInt" x="857.000000" y="118.000000"/> <Link from="node_isNil_1" output="OUTPUT" to="node_IF_1" input="COND"/> <Link from="node_Constant_1" output="VALUE" to="node_IF_1" input="THEN"/> --- 328,344 ---- <Node name="node_Min_2" type="Min" x="-519.000000" y="76.000000"/> <Node name="node_Min_3" type="Min" x="-400.000000" y="26.000000"/> ! <Node name="node_NOP_7" type="NOP" x="1184.000000" y="114.000000"/> ! <Node name="node_ToInt_3" type="ToInt" x="1115.000000" y="113.000000"/> ! <Node name="node_Add_3" type="Add" x="666.000000" y="-300.000000"/> ! <Node name="node_NOP_8" type="NOP" x="515.000000" y="-364.000000"/> ! <Node name="node_NOP_9" type="NOP" x="514.000000" y="-219.000000"/> ! <Node name="node_Add_4" type="Add" x="654.000000" y="-188.000000"/> ! <Node name="node_NOP_10" type="NOP" x="102.000000" y="-235.000000"/> ! <Node name="node_AUTO_SCALE_1" type="AUTO_SCALE" x="386.000000" y="-293.000000"/> ! <Node name="node_AUTO_SCALE_2" type="AUTO_SCALE" x="384.000000" y="-176.000000"/> ! <Node name="node_Div_1" type="Div" x="555.000000" y="135.000000"/> ! <Node name="node_ZOOM_FACTOR_1" type="ZOOM_FACTOR" x="58.000000" y="242.000000"/> ! <Node name="node_Div_2" type="Div" x="313.000000" y="316.000000"/> ! <Node name="node_Add_5" type="Add" x="799.000000" y="128.000000"/> <Link from="node_isNil_1" output="OUTPUT" to="node_IF_1" input="COND"/> <Link from="node_Constant_1" output="VALUE" to="node_IF_1" input="THEN"/> *************** *** 338,349 **** <Link from="node_Constant_5" output="VALUE" to="node_GenericPID_1" input="RESET"/> <Link from="node_Constant_6" output="VALUE" to="node_GenericPID_2" input="RESET"/> - <Link from="node_GenericPID_1" output="OUTPUT" to="node_ToInt_1" input="INPUT"/> - <Link from="node_GenericPID_2" output="OUTPUT" to="node_ToInt_2" input="INPUT"/> - <Link from="node_ToInt_2" output="OUTPUT" to="node_NOP_3" input="INPUT"/> <Link from="node_NOP_1" output="OUTPUT" to="node_GetVisualROIParam_1" input="IN_ROI"/> <Link from="node_GetVisualROIParam_1" output="CENTER_X" to="node_isNil_1" input="INPUT">-802.5 -155 -766 -154 -765 -320 -555 -320 </Link> <Link from="node_ToFloat_1" output="OUTPUT" to="node_IF_1" input="ELSE">-379 -272 -340 -272 -340 -290 -318 -291 </Link> <Link from="node_ToFloat_2" output="OUTPUT" to="node_IF_2" input="ELSE">-387 -160 -341 -161 -342 -175 -322 -176 </Link> - <Link from="node_ToInt_1" output="OUTPUT" to="node_NOP_2" input="INPUT"/> <Link from="node_Sub_1" output="OUTPUT" to="node_ToFloat_1" input="INPUT"/> <Link from="node_Constant_4" output="VALUE" to="node_Sub_1" input="INPUT2"/> --- 348,355 ---- *************** *** 353,357 **** <Link from="node_GetVisualROIParam_1" output="CENTER_Y" to="node_Sub_2" input="INPUT1">-802.5 -140 -746 -139 -747 -166 -561.5 -167.5 </Link> <Link from="node_AND_2" output="OUTPUT" to="node_IF_5" input="COND"/> - <Link from="node_Constant_15" output="VALUE" to="node_IF_5" input="THEN"/> <Link from="node_IF_6" output="OUTPUT" to="node_IF_5" input="ELSE">435 250 575 250 574 151 611 150 </Link> <Link from="node_Smaller_1" output="OUTPUT" to="node_AND_1" input="INPUT1"/> --- 359,362 ---- *************** *** 365,369 **** <Link from="node_Greater_3" output="OUTPUT" to="node_IF_6" input="COND">300.5 56 327 55 327 234 370 235 </Link> <Link from="node_Constant_14" output="VALUE" to="node_IF_6" input="THEN"/> - <Link from="node_Constant_13" output="VALUE" to="node_IF_6" input="ELSE"/> <Link from="node_NOP_5" output="OUTPUT" to="node_ABS_2" input="INPUT"/> <Link from="node_NOP_4" output="OUTPUT" to="node_ABS_1" input="INPUT"/> --- 370,373 ---- *************** *** 387,391 **** <Link from="node_Min_2" output="OUTPUT" to="node_Min_3" input="INPUT2">-503 76 -489 75 -489 34 -470 33.5 </Link> <Link from="node_Min_3" output="OUTPUT" to="node_NOP_6" input="INPUT"/> - <Link from="node_IF_5" output="OUTPUT" to="node_IF_4" input="ELSE"/> <Link from="node_IF_4" output="OUTPUT" to="node_ToInt_3" input="INPUT"/> <Link from="node_ToInt_3" output="OUTPUT" to="node_NOP_7" input="INPUT"/> --- 391,394 ---- *************** *** 393,403 **** <Link from="node_IF_2" output="OUTPUT" to="node_GenericPID_2" input="EPSILON"/> <Link from="node_GetVisualROIParam_1" output="HALF_WIDTH" to="node_Sub_3" input="INPUT2">-801 -126 -774 -125 -773 -28 -708.5 -27.5 </Link> ! <Link from="node_isNil_3" output="OUTPUT" to="node_IF_4" input="COND">683 -91 711 -91 711 104 739 104 </Link> <Link from="node_Constant_10" output="VALUE" to="node_IF_4" input="THEN"/> <NetInput name="IN_ROI" node="node_NOP_1" terminal="INPUT" object_type="any" description="The input"/> ! <NetOutput name="REL_PAN" node="node_NOP_2" terminal="OUTPUT" object_type="any" description="The output = The input"/> ! <NetOutput name="REL_TILT" node="node_NOP_3" terminal="OUTPUT" object_type="any" description="The output = The input"/> ! <NetOutput name="REL_ZOOM" node="node_NOP_7" terminal="OUTPUT" object_type="any" description="The output = The input"/> <Note x="0" y="0" visible="0" text="Created with FlowDesigner 0.9.0"/> </Network> <Network type="subnet" name="ABS"> --- 396,434 ---- <Link from="node_IF_2" output="OUTPUT" to="node_GenericPID_2" input="EPSILON"/> <Link from="node_GetVisualROIParam_1" output="HALF_WIDTH" to="node_Sub_3" input="INPUT2">-801 -126 -774 -125 -773 -28 -708.5 -27.5 </Link> ! <Link from="node_isNil_3" output="OUTPUT" to="node_IF_4" input="COND">683 -91 711 -91 711 99 997 99 </Link> <Link from="node_Constant_10" output="VALUE" to="node_IF_4" input="THEN"/> + <Link from="node_Add_3" output="OUTPUT" to="node_NOP_2" input="INPUT"/> + <Link from="node_ToInt_1" output="OUTPUT" to="node_Add_3" input="INPUT2"/> + <Link from="node_NOP_8" output="OUTPUT" to="node_Add_3" input="INPUT1"/> + <Link from="node_Add_4" output="OUTPUT" to="node_NOP_3" input="INPUT"/> + <Link from="node_NOP_9" output="OUTPUT" to="node_Add_4" input="INPUT1"/> + <Link from="node_ToInt_2" output="OUTPUT" to="node_Add_4" input="INPUT2"/> + <Link from="node_AUTO_SCALE_1" output="OUTPUT" to="node_ToInt_1" input="INPUT"/> + <Link from="node_GenericPID_1" output="OUTPUT" to="node_AUTO_SCALE_1" input="INPUT"/> + <Link from="node_AUTO_SCALE_2" output="OUTPUT" to="node_ToInt_2" input="INPUT"/> + <Link from="node_GenericPID_2" output="OUTPUT" to="node_AUTO_SCALE_2" input="INPUT"/> + <Link from="node_NOP_10" output="OUTPUT" to="node_AUTO_SCALE_1" input="ABS_ZOOM"/> + <Link from="node_NOP_10" output="OUTPUT" to="node_AUTO_SCALE_2" input="ABS_ZOOM"/> + <Link from="node_Div_1" output="OUTPUT" to="node_IF_5" input="THEN"/> + <Link from="node_Constant_15" output="VALUE" to="node_Div_1" input="NUM"/> + <Link from="node_ZOOM_FACTOR_1" output="ZOOM_FACTOR" to="node_Div_1" input="DEN"/> + <Link from="node_NOP_10" output="OUTPUT" to="node_ZOOM_FACTOR_1" input="ABS_ZOOM"/> + <Link from="node_Div_2" output="OUTPUT" to="node_IF_6" input="ELSE"/> + <Link from="node_Constant_13" output="VALUE" to="node_Div_2" input="NUM"/> + <Link from="node_ZOOM_FACTOR_1" output="ZOOM_FACTOR" to="node_Div_2" input="DEN">112 243 113 323 260 323.5 </Link> + <Link from="node_Add_5" output="OUTPUT" to="node_IF_4" input="ELSE"/> + <Link from="node_IF_5" output="OUTPUT" to="node_Add_5" input="INPUT2"/> + <Link from="node_NOP_10" output="OUTPUT" to="node_Add_5" input="INPUT1"/> <NetInput name="IN_ROI" node="node_NOP_1" terminal="INPUT" object_type="any" description="The input"/> ! <NetOutput name="ABS_PAN" node="node_NOP_2" terminal="OUTPUT" object_type="any" description="The output = The input"/> ! <NetOutput name="ABS_TILT" node="node_NOP_3" terminal="OUTPUT" object_type="any" description="The output = The input"/> ! <NetOutput name="ABS_ZOOM" node="node_NOP_7" terminal="OUTPUT" object_type="any" description="The output = The input"/> ! <NetInput name="CURRENT_PAN" node="node_NOP_8" terminal="INPUT" object_type="any" description="The input"/> ! <NetInput name="CURRENT_TILT" node="node_NOP_9" terminal="INPUT" object_type="any" description="The input"/> ! <NetInput name="CURRENT_ZOOM" node="node_NOP_10" terminal="INPUT" object_type="any" description="The input"/> <Note x="0" y="0" visible="0" text="Created with FlowDesigner 0.9.0"/> + <Note x="-548" y="285" visible="1" text="IMAGE BORDER WIDTH"/> + <Note x="-498" y="-362" visible="1" text="DISTANCE FROM CENTER"/> + <Note x="-79" y="419" visible="1" text="IF CENTERED AND BORDER IS LARGE ZOOM"/> </Network> <Network type="subnet" name="ABS"> *************** *** 426,452 **** </Network> <Network type="subnet" name="AUTO_SCALE"> ! <Node name="node_NOP_2" type="NOP" x="-736.000000" y="-90.000000"/> ! <Node name="node_Power_1" type="Power" x="-273.000000" y="-85.000000"/> ! <Node name="node_Mul_1" type="Mul" x="-422.000000" y="-76.000000"/> ! <Node name="node_Constant_1" type="Constant" x="-588.000000" y="26.000000"> <Parameter name="VALUE" type="float" value="-1" description="The value"/> </Node> <Node name="node_NOP_3" type="NOP" x="102.000000" y="-140.000000"/> ! <Node name="node_Constant_2" type="Constant" x="-436.000000" y="-126.000000"> <Parameter name="VALUE" type="float" value="1.1" description="The value"/> </Node> <Node name="node_Mul_2" type="Mul" x="-56.000000" y="-140.000000"/> ! <Node name="node_Sub_1" type="Sub" x="-561.000000" y="-82.000000"/> ! <Node name="node_Constant_3" type="Constant" x="-720.000000" y="-50.000000"> <Parameter name="VALUE" type="float" value="1" description="The value"/> </Node> ! <Node name="node_Div_1" type="Div" x="-336.000000" y="-251.000000"/> ! <Node name="node_Constant_4" type="Constant" x="-450.000000" y="-217.000000"> <Parameter name="VALUE" type="float" value="683" description="The value"/> </Node> ! <Node name="node_Add_1" type="Add" x="-227.000000" y="-259.000000"/> ! <Node name="node_Constant_5" type="Constant" x="-383.000000" y="-311.000000"> <Parameter name="VALUE" type="float" value="1.0" description="The value"/> </Node> <Link from="node_Constant_2" output="VALUE" to="node_Power_1" input="BASE"/> <Link from="node_Mul_1" output="OUTPUT" to="node_Power_1" input="EXP"/> --- 457,484 ---- </Network> <Network type="subnet" name="AUTO_SCALE"> ! <Node name="node_NOP_2" type="NOP" x="-886.000000" y="-60.000000"/> ! <Node name="node_Power_1" type="Power" x="-195.000000" y="-55.000000"/> ! <Node name="node_Mul_1" type="Mul" x="-344.000000" y="-46.000000"/> ! <Node name="node_Constant_1" type="Constant" x="-510.000000" y="56.000000"> <Parameter name="VALUE" type="float" value="-1" description="The value"/> </Node> <Node name="node_NOP_3" type="NOP" x="102.000000" y="-140.000000"/> ! <Node name="node_Constant_2" type="Constant" x="-358.000000" y="-96.000000"> <Parameter name="VALUE" type="float" value="1.1" description="The value"/> </Node> <Node name="node_Mul_2" type="Mul" x="-56.000000" y="-140.000000"/> ! <Node name="node_Sub_1" type="Sub" x="-483.000000" y="-52.000000"/> ! <Node name="node_Constant_3" type="Constant" x="-642.000000" y="-20.000000"> <Parameter name="VALUE" type="float" value="1" description="The value"/> </Node> ! <Node name="node_Div_1" type="Div" x="-567.000000" y="-139.000000"/> ! <Node name="node_Constant_4" type="Constant" x="-681.000000" y="-105.000000"> <Parameter name="VALUE" type="float" value="683" description="The value"/> </Node> ! <Node name="node_Add_1" type="Add" x="-458.000000" y="-147.000000"/> ! <Node name="node_Constant_5" type="Constant" x="-614.000000" y="-199.000000"> <Parameter name="VALUE" type="float" value="1.0" description="The value"/> </Node> + <Node name="node_ZOOM_FACTOR_1" type="ZOOM_FACTOR" x="-745.000000" y="-60.000000"/> <Link from="node_Constant_2" output="VALUE" to="node_Power_1" input="BASE"/> <Link from="node_Mul_1" output="OUTPUT" to="node_Power_1" input="EXP"/> *************** *** 455,459 **** <Link from="node_Mul_2" output="OUTPUT" to="node_NOP_3" input="INPUT"/> <Link from="node_Power_1" output="OUTPUT" to="node_Mul_2" input="INPUT2"/> - <Link from="node_NOP_2" output="OUTPUT" to="node_Sub_1" input="INPUT1"/> <Link from="node_Constant_3" output="VALUE" to="node_Sub_1" input="INPUT2"/> <Link from="node_Constant_4" output="VALUE" to="node_Div_1" input="DEN"/> --- 487,490 ---- *************** *** 461,464 **** --- 492,497 ---- <Link from="node_Div_1" output="OUTPUT" to="node_Add_1" input="INPUT2"/> <Link from="node_Add_1" output="OUTPUT" to="node_Mul_2" input="INPUT1"/> + <Link from="node_NOP_2" output="OUTPUT" to="node_ZOOM_FACTOR_1" input="ABS_ZOOM"/> + <Link from="node_ZOOM_FACTOR_1" output="ZOOM_FACTOR" to="node_Sub_1" input="INPUT1"/> <NetOutput name="OUTPUT" node="node_NOP_3" terminal="OUTPUT" object_type="any" description="The output = The input"/> <NetInput name="INPUT" node="node_Div_1" terminal="NUM" object_type="any" description="The numerator"/> *************** *** 466,469 **** --- 499,518 ---- <Note x="0" y="0" visible="0" text="Created with FlowDesigner 0.9.0"/> </Network> + <Network type="subnet" name="ZOOM_FACTOR"> + <Node name="node_Div_1" type="Div" x="-790.000000" y="18.000000"/> + <Node name="node_Constant_1" type="Constant" x="-911.000000" y="43.000000"> + <Parameter name="VALUE" type="float" value="683" description="The value"/> + </Node> + <Node name="node_Add_1" type="Add" x="-682.000000" y="10.000000"/> + <Node name="node_Constant_2" type="Constant" x="-838.000000" y="-42.000000"> + <Parameter name="VALUE" type="float" value="1.0" description="The value"/> + </Node> + <Link from="node_Constant_1" output="VALUE" to="node_Div_1" input="DEN"/> + <Link from="node_Constant_2" output="VALUE" to="node_Add_1" input="INPUT1"/> + <Link from="node_Div_1" output="OUTPUT" to="node_Add_1" input="INPUT2"/> + <NetInput name="ABS_ZOOM" node="node_Div_1" terminal="NUM" object_type="any" description="The numerator"/> + <NetOutput name="ZOOM_FACTOR" node="node_Add_1" terminal="OUTPUT" object_type="any" description="Result of the addition"/> + <Note x="0" y="0" visible="0" text="Created with FlowDesigner 0.9.0"/> + </Network> <Parameter name="DEFAULT_ABS_PAN_COMMAND" type="int" value="0"/> <Parameter name="DEFAULT_ABS_TILT_COMMAND" type="int" value="-300"/> |
|
From: MagellanPro <mag...@us...> - 2005-06-30 19:16:52
|
Update of /cvsroot/robotflow/RobotFlow/Devices/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv30345 Modified Files: SNCRZ30RS232.cc Log Message: fixed relative command squash Index: SNCRZ30RS232.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Devices/src/SNCRZ30RS232.cc,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** SNCRZ30RS232.cc 29 Jun 2005 19:20:13 -0000 1.3 --- SNCRZ30RS232.cc 30 Jun 2005 19:16:43 -0000 1.4 *************** *** 161,164 **** --- 161,165 ---- int destructor_loop_counter; + //Functions void Send() *************** *** 177,190 **** m_sending.pop_front(); ! ! //unlock here to avoid blocking on write (long) ! pthread_mutex_unlock(&m_list_lock); ! // lock... wait for ack signal before sending... Unlock is done in receive() ! //pthread_mutex_lock(&m_ack_lock); // Wait for a buffer to be freed up by the cam ! //pseudosem_wait(&m_buffer_empty); ! ! --- 178,194 ---- m_sending.pop_front(); ! ! //unlock here to avoid blocking on write (long) ! pthread_mutex_unlock(&m_list_lock); ! ! //lock... wait for ack signal before sending... ! //Unlock is done in receive() ! //cerr<<"locking ack"<<endl; ! pthread_mutex_lock(&m_ack_lock); ! // Wait for a buffer to be freed up by the cam ! pseudosem_wait(&m_buffer_empty); ! //cerr<<"Sending...size"<<m_sending.size()<<endl; *************** *** 195,199 **** } - }//if !empty else --- 199,202 ---- *************** *** 207,210 **** --- 210,215 ---- void Receive() { + + unsigned char PanReplyMessage[2]; string message; *************** *** 219,222 **** --- 224,228 ---- //if you read something if(newnumread > 0){ + //cerr<<"Receiving one byte"<<endl; message += PanReplyMessage[readTillNow]; *************** *** 227,230 **** --- 233,237 ---- // Push the message into the list m_received.push_back(message); + //cerr<<"New full message received"<<endl; // Unlock pthread_mutex_unlock(&m_list_rec_lock); *************** *** 287,290 **** --- 294,298 ---- //reading camera data + while (!m_received.empty()) { *************** *** 300,303 **** --- 308,313 ---- m_zoom_position = message[2]<<12 | message[3]<<8 | message[4]<<4 | message[5]; + //cerr<<"Zoom inquiry reply"<<endl; + pthread_mutex_unlock(&m_ack_lock); pseudosem_post(&m_buffer_empty); *************** *** 327,330 **** --- 337,341 ---- pthread_mutex_unlock(&m_ack_lock); pseudosem_post(&m_buffer_empty); + //cerr<<"pan tilt inquiry reply"<<endl; } *************** *** 332,336 **** --- 343,349 ---- if ((message.size() == 3) && (message[1] == 0x41 || message[1] == 0x42)) { + //cerr<<"ACK1"<<endl; pthread_mutex_unlock(&m_ack_lock); + //pseudosem_post(&m_buffer_empty); } *************** *** 339,342 **** --- 352,356 ---- if ((message.size() == 3) && (message[1] == 0x51 || message[1] == 0x52)) { + //cerr<<"COMPLETION"<<endl; pseudosem_post(&m_buffer_empty); } *************** *** 345,348 **** --- 359,363 ---- if ((message.size() == 5) && (message[1] == 0x01 && message[2] == 0x00 && message[3] == 0x01)) { + //cerr<< "Clear interface"<<endl; pseudosem_post(&m_buffer_empty); pthread_mutex_unlock(&m_ack_lock); *************** *** 352,356 **** if ((message.size() == 4) && (message[2] == 0x02)) { ! cerr << "Erreur de syntax" << endl; pthread_mutex_unlock(&m_ack_lock); pseudosem_post(&m_buffer_empty); --- 367,371 ---- if ((message.size() == 4) && (message[2] == 0x02)) { ! //cerr << "Erreur de syntax" << endl; pthread_mutex_unlock(&m_ack_lock); pseudosem_post(&m_buffer_empty); *************** *** 360,364 **** if ((message.size() == 4) && (message[2] == 0x03)) { ! cerr << "Command buffer full" << endl; pthread_mutex_unlock(&m_ack_lock); pseudosem_post(&m_buffer_empty); --- 375,379 ---- if ((message.size() == 4) && (message[2] == 0x03)) { ! //cerr << "Command buffer full" << endl; pthread_mutex_unlock(&m_ack_lock); pseudosem_post(&m_buffer_empty); *************** *** 367,371 **** if ((message.size() == 4) && (message[2] == 0x04)) { ! cerr << "Command cancel" << endl; pthread_mutex_unlock(&m_ack_lock); pseudosem_post(&m_buffer_empty); --- 382,386 ---- if ((message.size() == 4) && (message[2] == 0x04)) { ! //cerr << "Command cancel" << endl; pthread_mutex_unlock(&m_ack_lock); pseudosem_post(&m_buffer_empty); *************** *** 374,378 **** if ((message.size() == 4) && (message[2] == 0x05)) { ! cerr << "No socket" << endl; pthread_mutex_unlock(&m_ack_lock); pseudosem_post(&m_buffer_empty); --- 389,393 ---- if ((message.size() == 4) && (message[2] == 0x05)) { ! //cerr << "No socket" << endl; pthread_mutex_unlock(&m_ack_lock); pseudosem_post(&m_buffer_empty); *************** *** 382,386 **** if ((message.size() == 4) && (message[2] == 0x41)) { ! cerr << "Command not executable" << endl; pthread_mutex_unlock(&m_ack_lock); pseudosem_post(&m_buffer_empty); --- 397,401 ---- if ((message.size() == 4) && (message[2] == 0x41)) { ! //cerr << "Command not executable" << endl; pthread_mutex_unlock(&m_ack_lock); pseudosem_post(&m_buffer_empty); *************** *** 406,416 **** // If this is an ABS command replace it by the new one ! if(((*i)[1] == msg_to_send[1]) && ((*i)[2] == msg_to_send[2]) && ((*i)[3] == msg_to_send[3])){ ! // Replace the actual stuff with the new one. ! (*i) = msg_to_send; ! // Dont post any semaphore... ! duplicate_item_found = true; ! // Get out of here as soon as you can ! break; } } --- 421,462 ---- // If this is an ABS command replace it by the new one ! if ((*i).size() == msg_to_send.size()) { ! ! if(((*i)[0] == msg_to_send[0]) && ((*i)[1] == msg_to_send[1]) ! && ((*i)[2] == msg_to_send[2]) && ((*i)[3] == msg_to_send[3])){ ! ! //check for relative pan-tilt commands ! if ((unsigned char)(*i)[0] == 0x81 && (unsigned char)(*i)[1] == 0x01 ! && (unsigned char)(*i)[2] == 0x06 && (unsigned char)(*i)[3] == 0x03) { ! ! /* ! cerr<<"relative command detected in the list"<<endl; ! ! ! short pan_rel1, tilt_rel1, pan_rel2, tilt_rel2 = 0; ! ! ! pan_rel1 |= ((unsigned short)(*i)[6] << 12)& 0xF000; ! pan_rel1 |= ((unsigned short)(*i)[7] << 8) & 0x0F00; ! pan_rel1 |= ((unsigned short)(*i)[8] << 4) & 0x00F0; ! pan_rel1 |= (unsigned short)(*i)[9] & 0x000F; ! ! tilt_rel1 |= ((unsigned short)(*i)[10] << 12)& 0xF000; ! tilt_rel1 |= ((unsigned short)(*i)[11] << 8) & 0x0F00; ! tilt_rel1 |= ((unsigned short)(*i)[12] << 4) & 0x00F0; ! tilt_rel1 |= (unsigned short)(*i)[13] & 0x000F; ! ! cerr<<"detected pan rel "<<pan_rel1<<" tilt rel "<<tilt_rel1<<endl; ! */ ! ! } ! else { ! //Replace the actual stuff with the new one ! (*i) = msg_to_send; ! duplicate_item_found = true; ! break; ! ! } ! } } } *************** *** 461,465 **** message += 0x88; message += 0x01; ! message += (char)0x00; message += 0x01; message += 0xFF; --- 507,511 ---- message += 0x88; message += 0x01; ! message += (char) 0x00; message += 0x01; message += 0xFF; *************** *** 472,475 **** --- 518,524 ---- { + + //cerr<<"Abs pan "<<pan_position<<" Abs tilt "<<tilt_position<<endl; + string message; string last; *************** *** 502,506 **** message += 0xFF; ! // add it to the list dont worry be happy ! Add_message_to_send(message); } --- 551,555 ---- message += 0xFF; ! // add it to the list Add_message_to_send(message); } *************** *** 509,514 **** void Relative_movement(int pan_speed, int tilt_speed, int pan_position, int tilt_position) { ! //Absolute_movement(pan_speed,tilt_speed,m_pan_position + pan_position, m_tilt_position + tilt_position); ! string message; --- 558,562 ---- void Relative_movement(int pan_speed, int tilt_speed, int pan_position, int tilt_position) { ! //cerr<<"Rel pan "<<pan_position<<" Rel tilt "<<tilt_position<<endl; string message; *************** *** 546,550 **** ! // add it to the list dont worry be happy ! Add_message_to_send(message); --- 594,598 ---- ! // add it to the list Add_message_to_send(message); *************** *** 554,557 **** --- 602,607 ---- void Zoom(int data) { + //cerr<<"Zoom : "<<data<<endl; + string message; string last; *************** *** 583,586 **** --- 633,637 ---- void inquiry_pan_tilt() { + //cerr<<"inquiry pan tilt"<<endl; //pan + tilt request string PanTiltMessage; *************** *** 596,599 **** --- 647,652 ---- void inquiry_zoom(){ + + //cerr<<"inquiry zoom"<<endl; //zoom request string ZoomMessage; *************** *** 729,738 **** } inquiry_pan_tilt(); - inquiry_zoom(); ! //output position --- 782,792 ---- } + + inquiry_pan_tilt(); inquiry_zoom(); ! //output position |
|
From: Dominic L. <ma...@us...> - 2005-06-29 19:26:59
|
Update of /cvsroot/robotflow/RobotFlow/MARIE/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv5856 Modified Files: extractMarieCommandCamera.cpp Log Message: returning nilObject if MarieDataNull input Index: extractMarieCommandCamera.cpp =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/MARIE/src/extractMarieCommandCamera.cpp,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** extractMarieCommandCamera.cpp 29 Mar 2005 15:20:42 -0000 1.3 --- extractMarieCommandCamera.cpp 29 Jun 2005 19:26:51 -0000 1.4 *************** *** 137,209 **** try { ! RCPtr<MarieCommandCamera> command = getInput(m_commandCameraID,count); ! ! int relPan, relTilt, relZoom, relContrast, relBrightness = 0; ! int absPan, absTilt, absZoom, absContrast, absBrightness = 0; ! std::bitset<10> state = command->getState(); ! ! //get camera data ! relPan = command->getRelativePan(); ! relTilt = command->getRelativeTilt(); ! relZoom = command->getRelativeZoom(); ! relContrast = command->getRelativeContrast(); ! relBrightness = command->getRelativeBrightness(); ! ! absPan = command->getAbsolutePan(); ! absTilt = command->getAbsoluteTilt(); ! absZoom = command->getAbsoluteZoom(); ! absContrast = command->getAbsoluteContrast(); ! absBrightness = command->getAbsoluteBrightness(); ! ! ! if(state[CommandCamera::REL_PAN] == 1) ! (*outputs[m_relPanOutID].buffer)[count] = ObjectRef(Int::alloc(relPan)); ! else ! (*outputs[m_relPanOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::REL_TILT] == 1) ! (*outputs[m_relTiltOutID].buffer)[count] = ObjectRef(Int::alloc(relTilt)); ! else ! (*outputs[m_relTiltOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::REL_ZOOM] == 1) ! (*outputs[m_relZoomOutID].buffer)[count] = ObjectRef(Int::alloc(relZoom)); ! else ! (*outputs[m_relZoomOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::REL_BRIGHTNESS] == 1) ! (*outputs[m_relBrightnessOutID].buffer)[count] = ObjectRef(Int::alloc(relBrightness)); ! else ! (*outputs[m_relBrightnessOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::REL_CONTRAST] == 1) ! (*outputs[m_relContrastOutID].buffer)[count] = ObjectRef(Int::alloc(relContrast)); ! else ! (*outputs[m_relContrastOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::ABS_PAN] == 1) ! (*outputs[m_absPanOutID].buffer)[count] = ObjectRef(Int::alloc(absPan)); ! else ! (*outputs[m_absPanOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::ABS_TILT] == 1) ! (*outputs[m_absTiltOutID].buffer)[count] = ObjectRef(Int::alloc(absTilt)); ! else ! (*outputs[m_absTiltOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::ABS_ZOOM] == 1) ! (*outputs[m_absZoomOutID].buffer)[count] = ObjectRef(Int::alloc(absZoom)); ! else ! (*outputs[m_absZoomOutID].buffer)[count] = nilObject; ! if(state[CommandCamera::ABS_BRIGHTNESS] == 1) ! (*outputs[m_absBrightnessOutID].buffer)[count] = ObjectRef(Int::alloc(absBrightness)); ! else ! (*outputs[m_absBrightnessOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::ABS_CONTRAST] == 1) ! (*outputs[m_absContrastOutID].buffer)[count] = ObjectRef(Int::alloc(absContrast)); ! else ! (*outputs[m_absContrastOutID].buffer)[count] = nilObject; } catch (BaseException *e) --- 137,217 ---- try { ! ObjectRef inputValue = getInput(m_commandCameraID,count); ! if (!inputValue->isNil()) { ! ! RCPtr<MarieCommandCamera> command = inputValue; ! ! int relPan, relTilt, relZoom, relContrast, relBrightness = 0; ! int absPan, absTilt, absZoom, absContrast, absBrightness = 0; ! std::bitset<10> state = command->getState(); ! ! //get camera data ! relPan = command->getRelativePan(); ! relTilt = command->getRelativeTilt(); ! relZoom = command->getRelativeZoom(); ! relContrast = command->getRelativeContrast(); ! relBrightness = command->getRelativeBrightness(); ! ! absPan = command->getAbsolutePan(); ! absTilt = command->getAbsoluteTilt(); ! absZoom = command->getAbsoluteZoom(); ! absContrast = command->getAbsoluteContrast(); ! absBrightness = command->getAbsoluteBrightness(); ! ! ! if(state[CommandCamera::REL_PAN] == 1) ! (*outputs[m_relPanOutID].buffer)[count] = ObjectRef(Int::alloc(relPan)); ! else ! (*outputs[m_relPanOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::REL_TILT] == 1) ! (*outputs[m_relTiltOutID].buffer)[count] = ObjectRef(Int::alloc(relTilt)); ! else ! (*outputs[m_relTiltOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::REL_ZOOM] == 1) ! (*outputs[m_relZoomOutID].buffer)[count] = ObjectRef(Int::alloc(relZoom)); ! else ! (*outputs[m_relZoomOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::REL_BRIGHTNESS] == 1) ! (*outputs[m_relBrightnessOutID].buffer)[count] = ObjectRef(Int::alloc(relBrightness)); ! else ! (*outputs[m_relBrightnessOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::REL_CONTRAST] == 1) ! (*outputs[m_relContrastOutID].buffer)[count] = ObjectRef(Int::alloc(relContrast)); ! else ! (*outputs[m_relContrastOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::ABS_PAN] == 1) ! (*outputs[m_absPanOutID].buffer)[count] = ObjectRef(Int::alloc(absPan)); ! else ! (*outputs[m_absPanOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::ABS_TILT] == 1) ! (*outputs[m_absTiltOutID].buffer)[count] = ObjectRef(Int::alloc(absTilt)); ! else ! (*outputs[m_absTiltOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::ABS_ZOOM] == 1) ! (*outputs[m_absZoomOutID].buffer)[count] = ObjectRef(Int::alloc(absZoom)); ! else ! (*outputs[m_absZoomOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::ABS_BRIGHTNESS] == 1) ! (*outputs[m_absBrightnessOutID].buffer)[count] = ObjectRef(Int::alloc(absBrightness)); ! else ! (*outputs[m_absBrightnessOutID].buffer)[count] = nilObject; ! ! if(state[CommandCamera::ABS_CONTRAST] == 1) ! (*outputs[m_absContrastOutID].buffer)[count] = ObjectRef(Int::alloc(absContrast)); ! else ! (*outputs[m_absContrastOutID].buffer)[count] = nilObject; ! } ! else { ! out[count] = nilObject; ! } } catch (BaseException *e) |
|
From: MagellanPro <mag...@us...> - 2005-06-29 19:20:27
|
Update of /cvsroot/robotflow/RobotFlow/Devices/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv3112 Modified Files: SNCRZ30RS232.cc Log Message: avoiding waiting on ack receive mutex Index: SNCRZ30RS232.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Devices/src/SNCRZ30RS232.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** SNCRZ30RS232.cc 28 Jun 2005 19:31:57 -0000 1.2 --- SNCRZ30RS232.cc 29 Jun 2005 19:20:13 -0000 1.3 *************** *** 182,188 **** // lock... wait for ack signal before sending... Unlock is done in receive() ! pthread_mutex_lock(&m_ack_lock); // Wait for a buffer to be freed up by the cam ! pseudosem_wait(&m_buffer_empty); --- 182,188 ---- // lock... wait for ack signal before sending... Unlock is done in receive() ! //pthread_mutex_lock(&m_ack_lock); // Wait for a buffer to be freed up by the cam ! //pseudosem_wait(&m_buffer_empty); |
