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: clement r. <kl...@us...> - 2006-01-18 23:20:31
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv31823/src Modified Files: AvoidUtil.cc Log Message: modify behavior when going backward. We need to avoid if an obstacle is in the reverse security margin even if we are going backward. Index: AvoidUtil.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidUtil.cc,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** AvoidUtil.cc 20 Dec 2005 21:22:42 -0000 1.3 --- AvoidUtil.cc 18 Jan 2006 23:20:22 -0000 1.4 *************** *** 260,264 **** else { ! // If position == 9 (straight forward) if(position == 9) { --- 260,264 ---- else { ! // If position == 9 (straight backward) if(position == 9) { *************** *** 383,387 **** else { ! return false; } } --- 383,398 ---- else { ! // if we are going backward because we are too near from an obstacle ! findSmallestProximityMarginAndPosition(1, 4, minMargin1, minMarginPos1); ! if (minMargin1 <= m_reverseVelSecurityMargin) ! { ! pos = minMarginPos1; ! margin = minMargin1; ! return true; ! } ! else ! { ! return false; ! } } } *************** *** 404,411 **** //Reverse velocity vel = static_cast<int>(m_reverseSlope * margin + m_reverseB); - if(velocity<0) - { - vel *= -1; - } } else if(margin <= m_zeroVelSecurityMargin) --- 415,418 ---- *************** *** 418,425 **** // Evaluate linear approximation variable vel = static_cast<int>(m_velSlope * margin + m_velB); - if(velocity<0) - { - vel *= -1; - } } return vel; --- 425,428 ---- *************** *** 434,438 **** } ! // Evaluation de rotational velocity according to the margin int rot=0; --- 437,441 ---- } ! // Evaluation of rotational velocity according to the margin int rot=0; |
|
From: clement r. <kl...@us...> - 2006-01-18 23:15:42
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src/ut In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv30981/src/ut Removed Files: utAvoid Log Message: remove executable file from cvs --- utAvoid DELETED --- |
|
From: Fernyqc <fe...@us...> - 2006-01-18 22:07:38
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src/ut In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv6919/Behaviors/src/ut Added Files: AvoidTest.cc AvoidTest.h gcc.txt utAvoid ut.cc Log Message: Unit test for the utility class AvoidUtil --- NEW FILE: gcc.txt --- g++ -Wall -Werror -o utAvoid -I/home/fernyqc/robot/src/RobotFlow/Behaviors/include -lcppunit -ldl ut.cc AvoidTest.cc ../AvoidUtil.cc --- NEW FILE: utAvoid --- (This appears to be a binary file; contents omitted.) --- NEW FILE: AvoidTest.h --- // Prevent Multiple Inclusion #ifndef AVOID_TEST_H #define AVOID_TEST_H // Include Files #include <cppunit/extensions/HelperMacros.h> //Forward Declarations class AvoidUtil; class AvoidTest : public CppUnit::TestFixture { CPPUNIT_TEST_SUITE(AvoidTest); CPPUNIT_TEST(testInvalidConstructors); CPPUNIT_TEST(testValidConstructor); CPPUNIT_TEST(testEvaluateLongAvoidingThres); CPPUNIT_TEST(testEvaluateEllipticalAvoidingThres); CPPUNIT_TEST(testCreateEllipticalAvoidingThresBelt); CPPUNIT_TEST(testGetEllipticalAvoidingThresBelt); CPPUNIT_TEST(testCreateProximityMarginBelt); CPPUNIT_TEST(testGetProximityMarginBelt); CPPUNIT_TEST(testFindSmallestProximityMarginAndPosition); CPPUNIT_TEST(testIsAvoidanceNeeded); CPPUNIT_TEST(testEvalRotationalDirection); //CPPUNIT_TEST(testEvaluateEllipticalDirectionChangeThres); //CPPUNIT_TEST(testCreateEllipticalDirectionChangeThresBelt); //CPPUNIT_TEST(testGetEllipticalDirectionChangeThresBelt); //CPPUNIT_TEST(testIsDirectionChangeNeeded); CPPUNIT_TEST(testEvalAvoidingLinVel); CPPUNIT_TEST(testEvalAvoidingRotVel); //CPPUNIT_TEST(testEvalRotationalDirection); CPPUNIT_TEST_SUITE_END(); public: void setUp(); void tearDown(); void testInvalidConstructors(); void testValidConstructor(); void testEvaluateLongAvoidingThres(); void testEvaluateEllipticalAvoidingThres(); void testCreateEllipticalAvoidingThresBelt(); void testGetEllipticalAvoidingThresBelt(); void testCreateProximityMarginBelt(); void testGetProximityMarginBelt(); void testFindSmallestProximityMarginAndPosition(); void testIsAvoidanceNeeded(); void testEvalRotationalDirection(); //void testEvaluateEllipticalDirectionChangeThres(); //void testCreateEllipticalDirectionChangeThresBelt(); //void testGetEllipticalDirectionChangeThresBelt(); //void testIsDirectionChangeNeeded(); void testEvalAvoidingLinVel(); void testEvalAvoidingRotVel(); private: AvoidUtil* m_avoidUtil; }; #endif --- NEW FILE: ut.cc --- #include <cppunit/CompilerOutputter.h> #include <cppunit/extensions/TestFactoryRegistry.h> #include <cppunit/ui/text/TestRunner.h> int main(int argc, char* argv[]) { // Get the top level suite from the registry CppUnit::Test* suite = CppUnit::TestFactoryRegistry::getRegistry().makeTest(); // Adds the test to the list of test to run CppUnit::TextUi::TestRunner runner; runner.addTest(suite); // Change the default outputter to a compiler error format outputter runner.setOutputter(new CppUnit::CompilerOutputter(&runner.result(), std::cerr )); // Run the tests. bool wasSucessful = runner.run(); return wasSucessful; } --- NEW FILE: AvoidTest.cc --- #include "AvoidTest.h" #include "AvoidUtil.h" #include <iostream> CPPUNIT_TEST_SUITE_REGISTRATION(AvoidTest); // Virtual fonction implementation for cppunit creating an member instance of AvoidUtil void AvoidTest::setUp() { m_avoidUtil = new AvoidUtil(300, 100, 600, 100, 0.5, 0.3, 100, 0.5, 20, 3); } // Virtual fonction implementation for cppunit deleting the member instance of AvoidUtil void AvoidTest::tearDown() { delete m_avoidUtil; } // Test different invalid constructors void AvoidTest::testInvalidConstructors() { // velocity == 0 try { AvoidUtil avoid(0, 100, 1000, 100, 0.5, 0.3, 100, 0.2, 20, 3); CPPUNIT_ASSERT(0); } catch(AvoidUtilInvalidConstructorArguments) { CPPUNIT_ASSERT(1); } // ellipseLong == 0 try { AvoidUtil avoid(300, 0, 0, 100, 0.5, 0.3, 100, 0.2, 20, 3); CPPUNIT_ASSERT(0); } catch(AvoidUtilInvalidConstructorArguments) { CPPUNIT_ASSERT(1); } // ellipseLat == 0 try { AvoidUtil avoid(300, 0, 100, 0, 0.5, 0.3, 100, 0.2, 20, 3); CPPUNIT_ASSERT(0); } catch(AvoidUtilInvalidConstructorArguments) { CPPUNIT_ASSERT(1); } // ellipseLat > ellipseLong try { AvoidUtil avoid(300, 0, 10, 100, 0.5, 0.3, 100, 0.2, 20, 3); CPPUNIT_ASSERT(0); } catch(AvoidUtilInvalidConstructorArguments) { CPPUNIT_ASSERT(1); } // minLongEllipticalThres > ellipseLong try { AvoidUtil avoid(300, 1001, 1000, 100, 0.5, 0.3, 100, 0.2, 20, 3); CPPUNIT_ASSERT(0); } catch(AvoidUtilInvalidConstructorArguments) { CPPUNIT_ASSERT(1); } // Test a completer } // Test a valid constructor void AvoidTest::testValidConstructor() { try { AvoidUtil avoid1(300, 100, 600, 100, 0.5, 0.3, 100, 0.2, 20, 3); CPPUNIT_ASSERT(1); } catch(AvoidUtilInvalidConstructorArguments) { CPPUNIT_ASSERT(0); return; } } // Test the evaluation of the longitudinal avoiding distance threshold according to the speed of the robot void AvoidTest::testEvaluateLongAvoidingThres() { CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(100),m_avoidUtil->evalLongAvoidingThres(0)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(100),m_avoidUtil->evalLongAvoidingThres(49)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(100),m_avoidUtil->evalLongAvoidingThres(50)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(102),m_avoidUtil->evalLongAvoidingThres(51)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(600),m_avoidUtil->evalLongAvoidingThres(300)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(100),m_avoidUtil->evalLongAvoidingThres(-10)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(100),m_avoidUtil->evalLongAvoidingThres(-50)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(600),m_avoidUtil->evalLongAvoidingThres(-300)); AvoidUtil avoid(600, 800, 5000, 1000, 0.5, 0.2, 100, 0.3, 20, 3); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(5000),avoid.evalLongAvoidingThres(600)); } // Test the evaluation of the avoiding distance threshold on an ellipse at different angle void AvoidTest::testEvaluateEllipticalAvoidingThres() { CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(100),m_avoidUtil->evalEllipticalAvoidingThres(0, 600)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(600),m_avoidUtil->evalEllipticalAvoidingThres(90, 600)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(100),m_avoidUtil->evalEllipticalAvoidingThres(180, 600)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(600),m_avoidUtil->evalEllipticalAvoidingThres(270, 600)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(100),m_avoidUtil->evalEllipticalAvoidingThres(360, 600)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(100),m_avoidUtil->evalEllipticalAvoidingThres(540, 600)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(139),m_avoidUtil->evalEllipticalAvoidingThres(45, 600)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(192),m_avoidUtil->evalEllipticalAvoidingThres(120, 600)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(192),m_avoidUtil->evalEllipticalAvoidingThres(240, 600)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(103),m_avoidUtil->evalEllipticalAvoidingThres(345, 600)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(1000),m_avoidUtil->evalEllipticalAvoidingThres(90, 1000)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(300),m_avoidUtil->evalEllipticalAvoidingThres(90, 300)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(166),m_avoidUtil->evalEllipticalAvoidingThres(0, 1000)); CPPUNIT_ASSERT_EQUAL(static_cast<unsigned int>(50),m_avoidUtil->evalEllipticalAvoidingThres(0, 300)); // Singularity if distanceThres == 0 try { m_avoidUtil->evalEllipticalAvoidingThres(540, 0); CPPUNIT_ASSERT(0); } catch(AvoidUtilIllegalAvoidingThreshold) { CPPUNIT_ASSERT(1); } } // Test the condition of the creation of the elliptical avoiding thres belt void AvoidTest::testCreateEllipticalAvoidingThresBelt() { CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->createEllipticalAvoidingThresBelt(361, 600)); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->createEllipticalAvoidingThresBelt(0, 600)); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->createEllipticalAvoidingThresBelt(360, 0)); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->createEllipticalAvoidingThresBelt(360, 600)); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->createEllipticalAvoidingThresBelt(12, 600)); } // Test de construction of a belt of distance avoiding threshold on an ellipse void AvoidTest::testGetEllipticalAvoidingThresBelt() { // Manually build the theorical result of the vector std::vector<int> thresTheoVector(12); thresTheoVector[0] = 100; thresTheoVector[1] = 114; thresTheoVector[2] = 192; thresTheoVector[3] = 600; thresTheoVector[4] = 192; thresTheoVector[5] = 114; thresTheoVector[6] = 100; thresTheoVector[7] = 114; thresTheoVector[8] = 192; thresTheoVector[9] = 600; thresTheoVector[10] = 192; thresTheoVector[11] = 114; // Create the ellipse avoiding distance belt and pass the test m_avoidUtil->createEllipticalAvoidingThresBelt(12, 600); CPPUNIT_ASSERT_EQUAL(true, thresTheoVector == m_avoidUtil->getEllipticalAvoidingThresBelt()); } // Test the condition of construction of a proximity margin belt void AvoidTest::testCreateProximityMarginBelt() { // Build different range belt std::vector<int> rangeBelt1; std::vector<int> rangeBelt2(12, 500); std::vector<int> rangeBelt3(25, 500); m_avoidUtil->createEllipticalAvoidingThresBelt(12, 600); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->createProximityMarginBelt(rangeBelt1)); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->createProximityMarginBelt(rangeBelt2)); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->createProximityMarginBelt(rangeBelt3)); } // Test de construction of a proximity margin belt void AvoidTest::testGetProximityMarginBelt() { // Build a rangeBelt std::vector<int> rangeBelt(12, 500); // Build the theorical solution of the proximityMarginBelt std::vector<double> theoProximityBelt(12, 0); theoProximityBelt[0] = 500.0 / 100; theoProximityBelt[1] = 500.0 / 114; theoProximityBelt[2] = 500.0 / 192; theoProximityBelt[3] = 500.0 / 600; theoProximityBelt[4] = 500.0 / 192; theoProximityBelt[5] = 500.0 / 114; theoProximityBelt[6] = 500.0 / 100; theoProximityBelt[7] = 500.0 / 114; theoProximityBelt[8] = 500.0 / 192; theoProximityBelt[9] = 500.0 / 600; theoProximityBelt[10] = 500.0 / 192; theoProximityBelt[11] = 500.0 / 114; // Create the ellipse avoiding distance belt and pass the test m_avoidUtil->createEllipticalAvoidingThresBelt(12, 600); m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(true, theoProximityBelt == m_avoidUtil->getProximityMarginBelt()); // Another test rangeBelt.clear(); rangeBelt.resize(12, 50); // Build the theorical solution of the proximityMarginBelt theoProximityBelt.clear(); theoProximityBelt.resize(12, 0); theoProximityBelt[0] = 50.0 / 16; theoProximityBelt[1] = 50.0 / 19; theoProximityBelt[2] = 50.0 / 32; theoProximityBelt[3] = 50.0 / 100; theoProximityBelt[4] = 50.0 / 32; theoProximityBelt[5] = 50.0 / 19; theoProximityBelt[6] = 50.0 / 16; theoProximityBelt[7] = 50.0 / 19; theoProximityBelt[8] = 50.0 / 32; theoProximityBelt[9] = 50.0 / 100; theoProximityBelt[10] = 50.0 / 32; theoProximityBelt[11] = 50.0 / 19; // Create the ellipse avoiding distance belt and pass the test m_avoidUtil->createEllipticalAvoidingThresBelt(12, 100); m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(true, theoProximityBelt == m_avoidUtil->getProximityMarginBelt()); } // Test the search for the smallest proximity margin value and the position of that value function void AvoidTest::testFindSmallestProximityMarginAndPosition() { // Variable for the smallest margin and its position double smallestMargin; unsigned int position; // Create the proximity belt std::vector<int> rangeBelt(12, 500); m_avoidUtil->createEllipticalAvoidingThresBelt(12, 600); m_avoidUtil->createProximityMarginBelt(rangeBelt); // Exception -> parameter lPos > hPos try { m_avoidUtil->findSmallestProximityMarginAndPosition(3, 1, smallestMargin, position); CPPUNIT_ASSERT(0); } catch(AvoidUtilFindSmallestMarginException) { CPPUNIT_ASSERT(1); } // Exception -> index out of range try { m_avoidUtil->findSmallestProximityMarginAndPosition(0, 12, smallestMargin, position); CPPUNIT_ASSERT(0); } catch(AvoidUtilFindSmallestMarginException) { CPPUNIT_ASSERT(1); } // Normal utilization m_avoidUtil->findSmallestProximityMarginAndPosition(0, 11, smallestMargin, position); CPPUNIT_ASSERT_EQUAL(true, (smallestMargin == 500.0 / 600) && (position == 3)); m_avoidUtil->findSmallestProximityMarginAndPosition(4, 11, smallestMargin, position); CPPUNIT_ASSERT_EQUAL(true, (smallestMargin == 500.0 / 600) && (position == 9)); m_avoidUtil->findSmallestProximityMarginAndPosition(0, 2, smallestMargin, position); CPPUNIT_ASSERT_EQUAL(true, (smallestMargin == 500.0 / 192) && (position == 2)); m_avoidUtil->findSmallestProximityMarginAndPosition(0, 0, smallestMargin, position); CPPUNIT_ASSERT_EQUAL(true, (smallestMargin == 5.0) && (position == 0)); m_avoidUtil->findSmallestProximityMarginAndPosition(11, 11, smallestMargin, position); CPPUNIT_ASSERT_EQUAL(true, (smallestMargin == 500.0 / 114) && (position == 11)); // Second test rangeBelt.clear(); rangeBelt.resize(12, 101); rangeBelt[0] = 15; m_avoidUtil->createEllipticalAvoidingThresBelt(12, 100); m_avoidUtil->createProximityMarginBelt(rangeBelt); m_avoidUtil->findSmallestProximityMarginAndPosition(0, 11, smallestMargin, position); CPPUNIT_ASSERT_EQUAL(true, (smallestMargin == 15.0 / 16) && (position == 0)); //std::cout << std::endl << "small -> " << smallestMargin << std::endl; } // Test the evaluation of the rotational direction // Assuming forward direction void AvoidTest::testEvalRotationalDirection() { // Create the proximity belt std::vector<int> rangeBelt(12, 500); m_avoidUtil->createEllipticalAvoidingThresBelt(12, 1000); m_avoidUtil->createProximityMarginBelt(rangeBelt); // Velocity == 0 CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(0,0)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(0,1)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(0,2)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(0,3)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(0,4)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(0,5)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(0,6)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(0,7)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(0,8)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(0,9)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(0,10)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(0,11)); // Velocity > 0 CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(100,0)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(100,1)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(100,2)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(100,3)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(100,4)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(100,5)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(100,6)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(100,7)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(100,8)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(100,9)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(100,10)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(100,11)); // Velocity < 0 CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(-100,0)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(-100,1)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(-100,2)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(-100,3)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(-100,4)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(-100,5)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(-100,6)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(-100,7)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(-100,8)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(-100,9)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(-100,10)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(-100,11)); // Test the case when the obstacle is straight ahead. // Direction should be the highest proximity margin on the side of the robot rangeBelt[0] = 400; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(100,3)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(-100,9)); rangeBelt[0] = 600; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(100,3)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(-100,9)); // Test the memory on that particular situation of obstacle in front m_avoidUtil->evalRotationalDirection(100,3); rangeBelt[0] = 400; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(100,3)); rangeBelt[0] = 600; m_avoidUtil->createProximityMarginBelt(rangeBelt); m_avoidUtil->evalRotationalDirection(-100,9); rangeBelt[0] = 400; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(-100,9)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(-100,9)); CPPUNIT_ASSERT_EQUAL(-1, m_avoidUtil->evalRotationalDirection(-100,9)); CPPUNIT_ASSERT_EQUAL(1, m_avoidUtil->evalRotationalDirection(-100,9)); } // Test if avoidance is needed according to different rangeBelt void AvoidTest::testIsAvoidanceNeeded() { double margin=0.0; unsigned int pos=0; //------------------------- // Tests when velocity == 0 //------------------------- // Create the range belt without avoidance std::vector<int> rangeBelt(12, 101); m_avoidUtil->createEllipticalAvoidingThresBelt(12, 100); m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 0, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(0)); // Create the range belt at the limit of avoidance rangeBelt.clear(); rangeBelt.resize(12, 100); m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 0, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(0)); // Test different avoiding situation rangeBelt.clear(); rangeBelt.resize(12, 101); rangeBelt[0] = 17; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 0, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(0)); rangeBelt[0] = 10; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 0, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(0)); rangeBelt.clear(); rangeBelt.resize(12, 101); rangeBelt[3] = 101; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 0, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(0)); rangeBelt[3] = 99; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 0, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(3)); pos = 0; rangeBelt.clear(); rangeBelt.resize(12, 101); rangeBelt[5] = 20; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 0, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(0)); rangeBelt[5] = 18; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 0, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(5)); pos = 0; rangeBelt.clear(); rangeBelt.resize(12, 101); rangeBelt[10] = 33; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 0, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(0)); rangeBelt[10] = 31; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 0, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(10)); pos = 0; //------------------------- // Tests when velocity != 0 //------------------------- pos = 0; // Create the range belt without avoidance rangeBelt.clear(); rangeBelt.resize(12, 601); m_avoidUtil->createEllipticalAvoidingThresBelt(12, 600); m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(0)); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->isAvoidanceNeeded(rangeBelt, -600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(0)); // Create the range belt at the limit of avoidance rangeBelt.clear(); rangeBelt.resize(12, 600); m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(0)); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->isAvoidanceNeeded(rangeBelt, -600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(0)); // Avoid on 3 and 9 rangeBelt.clear(); rangeBelt.resize(12, 601); rangeBelt[3] = 599; rangeBelt[9] = 599; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(3)); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, -600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(9)); // Avoid on 2 and 8 rangeBelt.clear(); rangeBelt.resize(12, 601); rangeBelt[2] = 191; rangeBelt[8] = 191; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(2)); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, -600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(8)); // Avoid on 1 and 7 rangeBelt.clear(); rangeBelt.resize(12, 601); rangeBelt[1] = 113; rangeBelt[7] = 113; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(1)); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, -600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(7)); // Avoid on 0 and 6 rangeBelt.clear(); rangeBelt.resize(12, 601); rangeBelt[0] = 99; rangeBelt[6] = 99; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(0)); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, -600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(6)); // Avoid on 11 forward rangeBelt.clear(); rangeBelt.resize(12, 601); rangeBelt[11] = 113; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(11)); // Avoid on 5 backward rangeBelt.clear(); rangeBelt.resize(12, 601); rangeBelt[5] = 113; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, -600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(5)); // Avoid on 1 backward rangeBelt.clear(); rangeBelt.resize(12, 601); rangeBelt[1] = 113; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(true, m_avoidUtil->isAvoidanceNeeded(rangeBelt, -600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(1)); // No avoidance according to the direction of the robot pos = 0; rangeBelt.clear(); rangeBelt.resize(12, 601); rangeBelt[3] = 100; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->isAvoidanceNeeded(rangeBelt, -600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(0)); rangeBelt[3] = 601; rangeBelt[9] = 100; m_avoidUtil->createProximityMarginBelt(rangeBelt); CPPUNIT_ASSERT_EQUAL(false, m_avoidUtil->isAvoidanceNeeded(rangeBelt, 600, pos, margin)); CPPUNIT_ASSERT_EQUAL(pos, static_cast<unsigned int>(0)); } void AvoidTest::testEvalAvoidingLinVel() { // margin < 0 try { m_avoidUtil->evalAvoidingLinVel(0,-0.1); CPPUNIT_ASSERT(0); } catch(AvoidUtilMarginException) { CPPUNIT_ASSERT(1); } // margin > 1 try { m_avoidUtil->evalAvoidingLinVel(0,1.1); CPPUNIT_ASSERT(0); } catch(AvoidUtilMarginException) { CPPUNIT_ASSERT(1); } CPPUNIT_ASSERT_EQUAL(-100, m_avoidUtil->evalAvoidingLinVel(0,0)); CPPUNIT_ASSERT_EQUAL(-49, m_avoidUtil->evalAvoidingLinVel(0,0.15)); CPPUNIT_ASSERT_EQUAL(0, m_avoidUtil->evalAvoidingLinVel(0,0.3)); CPPUNIT_ASSERT_EQUAL(100, m_avoidUtil->evalAvoidingLinVel(-1,0)); CPPUNIT_ASSERT_EQUAL(49, m_avoidUtil->evalAvoidingLinVel(-1,0.15)); CPPUNIT_ASSERT_EQUAL(0, m_avoidUtil->evalAvoidingLinVel(-1,0.3)); CPPUNIT_ASSERT_EQUAL(0, m_avoidUtil->evalAvoidingLinVel(0,0.31)); CPPUNIT_ASSERT_EQUAL(0, m_avoidUtil->evalAvoidingLinVel(0,0.5)); CPPUNIT_ASSERT_EQUAL(6, m_avoidUtil->evalAvoidingLinVel(0,0.51)); CPPUNIT_ASSERT_EQUAL(300, m_avoidUtil->evalAvoidingLinVel(0,1)); CPPUNIT_ASSERT_EQUAL(300, m_avoidUtil->evalAvoidingLinVel(100,1)); CPPUNIT_ASSERT_EQUAL(150, m_avoidUtil->evalAvoidingLinVel(100,0.75)); CPPUNIT_ASSERT_EQUAL(6, m_avoidUtil->evalAvoidingLinVel(100,0.51)); CPPUNIT_ASSERT_EQUAL(-300, m_avoidUtil->evalAvoidingLinVel(-100,1)); CPPUNIT_ASSERT_EQUAL(-150, m_avoidUtil->evalAvoidingLinVel(-100,0.75)); CPPUNIT_ASSERT_EQUAL(-6, m_avoidUtil->evalAvoidingLinVel(-100,0.51)); } void AvoidTest::testEvalAvoidingRotVel() { // margin < 0 try { m_avoidUtil->evalAvoidingRotVel(0,0,-0.1); CPPUNIT_ASSERT(0); } catch(AvoidUtilMarginException) { CPPUNIT_ASSERT(1); } // margin > 1 try { m_avoidUtil->evalAvoidingRotVel(0,0,1.1); CPPUNIT_ASSERT(0); } catch(AvoidUtilMarginException) { CPPUNIT_ASSERT(1); } CPPUNIT_ASSERT_EQUAL(-20, m_avoidUtil->evalAvoidingRotVel(100, 0, 0)); CPPUNIT_ASSERT_EQUAL(0, m_avoidUtil->evalAvoidingRotVel(100, 0, 1)); CPPUNIT_ASSERT_EQUAL(10, m_avoidUtil->evalAvoidingRotVel(100, 0, 0.75)); } |
|
From: Fernyqc <fe...@us...> - 2006-01-18 22:06:49
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src/ut In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv6549/Behaviors/src/ut Log Message: Directory /cvsroot/robotflow/RobotFlow/Behaviors/src/ut added to the repository |
|
From: Fernyqc <fe...@us...> - 2005-12-20 21:24:30
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv7443/Behaviors/src Modified Files: EllipticalAvoid.cc Log Message: Elimination of the oscillation when detecting obstacle at front Index: EllipticalAvoid.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/EllipticalAvoid.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** EllipticalAvoid.cc 13 Dec 2005 14:47:17 -0000 1.2 --- EllipticalAvoid.cc 20 Dec 2005 21:24:22 -0000 1.3 *************** *** 182,186 **** m_reverseVel, m_rotVelSecurityMargin, ! m_rotVel); } catch (AvoidUtilInvalidConstructorArguments) --- 182,187 ---- m_reverseVel, m_rotVelSecurityMargin, ! m_rotVel, ! m_nbAvoidance); } catch (AvoidUtilInvalidConstructorArguments) *************** *** 196,203 **** void calculate_behavior(int output_id, int count, Buffer &out) { - //std::cout << "EllipticalAvoid : output_id -> " << output_id << std::endl; - //std::cout << "EllipticalAvoid -> getInput1" << std::endl; ObjectRef rangeValueRef = getInput(rangeID,count); - //std::cout << "EllipticalAvoid -> getInput2s" << std::endl; ObjectRef velInRef = getInput(velocityInID,count); --- 197,201 ---- *************** *** 208,212 **** static double lastMargin = -1; - //std::cout << "range, vel -> " << rangeValueRef->isNil() << "," << velInRef->isNil() << std::endl; if((!rangeValueRef->isNil()) && (!velInRef->isNil())){ // Cast the range belt --- 206,209 ---- *************** *** 258,269 **** } } ! /* ! std::cout << "avoid -> " << avoid << std::endl; ! std::cout << "avoidActivated -> " << avoidActivated << std::endl; ! std::cout << "memory -> " << memory << std::endl; ! std::cout << "velIn -> " << velIn << std::endl; ! std::cout << "Pos -> " << pos << std::endl; ! std::cout << "Margin -> " << margin << std::endl; ! */ if(avoid) { --- 255,259 ---- } } ! if(avoid) { *************** *** 279,283 **** (*outputs[velocityOutID].buffer)[count] = ObjectRef(Int::alloc(velOut)); (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(rotOut)); - std::cout << "velOut -> " << velOut << std::endl; } else --- 269,272 ---- *************** *** 286,290 **** (*outputs[velocityOutID].buffer)[count] = nilObject; (*outputs[rotationID].buffer)[count] = nilObject; - std::cout << "velOut -> nilObject" << std::endl; } } --- 275,278 ---- *************** *** 292,296 **** (*outputs[velocityOutID].buffer)[count] = nilObject; (*outputs[rotationID].buffer)[count] = nilObject; - std::cout << "inputs -> nilObject" << std::endl; } }//calculate_behavior --- 280,283 ---- |
|
From: Fernyqc <fe...@us...> - 2005-12-20 21:22:50
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv7128/Behaviors/src Modified Files: AvoidUtil.cc Log Message: Elimination of the oscillation when detecting obstacle at front Index: AvoidUtil.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidUtil.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** AvoidUtil.cc 13 Dec 2005 14:47:17 -0000 1.2 --- AvoidUtil.cc 20 Dec 2005 21:22:42 -0000 1.3 *************** *** 36,40 **** int reverseVel, double rotVelSecurityMargin, ! int rotVel) : m_typVelocity(typVelocity), m_minEllipseLong(minEllipseLong), --- 36,41 ---- int reverseVel, double rotVelSecurityMargin, ! int rotVel, ! unsigned int nbOscillation) : m_typVelocity(typVelocity), m_minEllipseLong(minEllipseLong), *************** *** 45,49 **** m_reverseVel(reverseVel), m_rotVelSecurityMargin(rotVelSecurityMargin), ! m_rotVel(rotVel) { // The velocity must not be o --- 46,51 ---- m_reverseVel(reverseVel), m_rotVelSecurityMargin(rotVelSecurityMargin), ! m_rotVel(rotVel), ! m_nbOscillation(nbOscillation) { // The velocity must not be o *************** *** 87,90 **** --- 89,96 ---- m_slopeRot = (0-m_rotVel) / (1-m_rotVelSecurityMargin); m_rotB = -1*m_slopeRot; + + // Initialization + m_lastPos = 0; + m_lastSign = 1; } *************** *** 160,176 **** // Fill the belt with the avoiding distance ! int min=0; for(unsigned int i = 0; i<beltSize; ++i) { m_ellipseBelt[i] = evalEllipticalAvoidingThres(i*angularIncrement, distanceThres); - // Check if the minimal safety protection is respected - if(m_minEllipseLong != 0) - { - min = evalEllipticalAvoidingThres(i*angularIncrement, m_minEllipseLong); - if(m_ellipseBelt[i] < min) - { - m_ellipseBelt[i] = min; - } - } } return true; --- 166,173 ---- // Fill the belt with the avoiding distance ! //int min=0; for(unsigned int i = 0; i<beltSize; ++i) { m_ellipseBelt[i] = evalEllipticalAvoidingThres(i*angularIncrement, distanceThres); } return true; *************** *** 183,214 **** } - /*bool AvoidUtil::createEllipticalDirectionChangeThresBelt(unsigned int beltSize) - { - if((beltSize == 0) or (beltSize > 360)) - { - // Beltsize must be between 1 and 360 degree and distanceThres <> 0 - return false; - } - else{ - // Determine the angular increment for the belt - unsigned int angularIncrement = 360 / beltSize; - - // Resize the std::Vector - m_directionChangeBelt.resize(beltSize); - - // Fill the belt with the avoiding distance - for(unsigned int i = 0; i < beltSize; ++i) - { - m_directionChangeBelt[i] = evalEllipticalDirectionChangeThres(i*angularIncrement); - } - return true; - } - } - - const std::vector<unsigned int>& AvoidUtil::getEllipticalDirectionChangeThresBelt() - { - return m_directionChangeBelt; - }*/ - bool AvoidUtil::createProximityMarginBelt(std::vector<int> rangeBelt) { --- 180,183 ---- *************** *** 315,318 **** --- 284,314 ---- } } + + // Override the sign computation to keep the last value in case of frontal oscillation + if((position == 3) || (position == 9)) + { + if(m_lastPos == position) + { + if(m_CurrentOscillation > 0) + { + sign = m_lastSign; + } + else + { + m_lastPos = 0; + } + --m_CurrentOscillation; + } + else + { + m_CurrentOscillation = m_nbOscillation; + m_lastPos = position; + m_lastSign = sign; + } + } + else + { + m_lastPos = position; + } return sign; } *************** *** 393,408 **** return false; } - /* - bool AvoidUtil::isDirectionChangeNeeded(unsigned int position, unsigned int rangeValue) - { - if(position >= m_directionChangeBelt.size()) - { - throw AvoidUtilBeltPositionOutOfRange(); - } - else - { - return (rangeValue < m_directionChangeBelt[position]); - } - }*/ int AvoidUtil::evalAvoidingLinVel(int velocity, double margin) --- 389,392 ---- *************** *** 434,437 **** --- 418,425 ---- // Evaluate linear approximation variable vel = static_cast<int>(m_velSlope * margin + m_velB); + if(velocity<0) + { + vel *= -1; + } } return vel; |
|
From: Fernyqc <fe...@us...> - 2005-12-20 21:22:50
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv7128/Behaviors/include Modified Files: AvoidUtil.h Log Message: Elimination of the oscillation when detecting obstacle at front Index: AvoidUtil.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/include/AvoidUtil.h,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** AvoidUtil.h 13 Dec 2005 15:12:56 -0000 1.2 --- AvoidUtil.h 20 Dec 2005 21:22:42 -0000 1.3 *************** *** 42,46 **** int reverseVel, double rotVelSecurityMargin, ! int rotVel); ~AvoidUtil(); unsigned int evalLongAvoidingThres(int velocity) const; --- 42,47 ---- int reverseVel, double rotVelSecurityMargin, ! int rotVel, ! unsigned int nbOscillation); ~AvoidUtil(); unsigned int evalLongAvoidingThres(int velocity) const; *************** *** 53,60 **** bool isAvoidanceNeeded(std::vector<int> rangeBelt, int velocity, unsigned int &pos, double &margin); int evalRotationalDirection(int velocity, unsigned int position); - //unsigned int evalEllipticalDirectionChangeThres(unsigned int theta) const; - // bool createEllipticalDirectionChangeThresBelt(unsigned int beltSize); - //const std::vector<unsigned int>& getEllipticalDirectionChangeThresBelt(); - //bool isDirectionChangeNeeded(unsigned int position, unsigned int rangeValue); int evalAvoidingLinVel(int velocity, double margin); int evalAvoidingRotVel(int velocity, unsigned int pos, double margin); --- 54,57 ---- *************** *** 74,79 **** --- 71,80 ---- double m_rotVelSecurityMargin; int m_rotVel; + unsigned int m_nbOscillation; // Variables + unsigned int m_lastPos; + int m_lastSign; + unsigned int m_CurrentOscillation; double m_slope; double m_velSlope; |
|
From: clement r. <kl...@us...> - 2005-12-13 16:56:04
|
Update of /cvsroot/robotflow/RobotFlow/Sensors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv24546 Modified Files: RangeFashion.cc Log Message: remove dead zones of range fashion Index: RangeFashion.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Sensors/src/RangeFashion.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** RangeFashion.cc 12 Dec 2005 18:20:23 -0000 1.2 --- RangeFashion.cc 13 Dec 2005 16:55:50 -0000 1.3 *************** *** 136,141 **** { // 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; --- 136,141 ---- { // Determine the range for the position ! int begin = 60 * i - 30; ! int end = 60 * i + 30; if (i==0) begin = 0; else if (i==6) end = 360; |
|
From: Fernyqc <fe...@us...> - 2005-12-13 15:13:04
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv4325 Modified Files: AvoidUtil.h Log Message: Header including the addition of the TYPICAL_AVOIDANCE_VELOCITY Index: AvoidUtil.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/include/AvoidUtil.h,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** AvoidUtil.h 12 Dec 2005 18:28:46 -0000 1.1 --- AvoidUtil.h 13 Dec 2005 15:12:56 -0000 1.2 *************** *** 34,38 **** { public: ! AvoidUtil(unsigned int maxVelocity, unsigned int minEllipseLong, unsigned int ellipseLong, --- 34,38 ---- { public: ! AvoidUtil(unsigned int typVelocity, unsigned int minEllipseLong, unsigned int ellipseLong, *************** *** 65,69 **** // Constructor arguments ! unsigned int m_maxVelocity; unsigned int m_minEllipseLong; unsigned int m_ellipseLong; --- 65,69 ---- // Constructor arguments ! unsigned int m_typVelocity; unsigned int m_minEllipseLong; unsigned int m_ellipseLong; *************** *** 77,80 **** --- 77,82 ---- // Variables double m_slope; + double m_velSlope; + double m_velB; double m_reverseSlope; double m_reverseB; |
|
From: Fernyqc <fe...@us...> - 2005-12-13 14:47:28
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv31472 Modified Files: AvoidUtil.cc EllipticalAvoid.cc GotoPat.cc Log Message: Addition of the memory on the EllipticalAvoid behavior; Addition of the TYPICAL_VELOCITY_AVOID principle; Index: EllipticalAvoid.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/EllipticalAvoid.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** EllipticalAvoid.cc 12 Dec 2005 18:27:49 -0000 1.1 --- EllipticalAvoid.cc 13 Dec 2005 14:47:17 -0000 1.2 *************** *** 43,47 **** * * @input_name RANGE_BELT ! * @input_type Vector<unsigned int> * @input_description Range mesure around the robot * --- 43,47 ---- * * @input_name RANGE_BELT ! * @input_type Vector<int> * @input_description Range mesure around the robot * *************** *** 61,70 **** * @output_description Behavior exploitation. * ! * @parameter_name MAX_VELOCITY * @parameter_type int ! * @parameter_value 600 ! * @parameter_description Robot maximal forward velocity * ! * @parameter_name MIN_ELLIPSE_LONG * @parameter_type int * @parameter_value 800 --- 61,70 ---- * @output_description Behavior exploitation. * ! * @parameter_name TYPICAL_AVOIDANCE_VELOCITY * @parameter_type int ! * @parameter_value 300 ! * @parameter_description Typical velocity to apply when avoiding (mm/s) * ! * @parameter_name MIN_ELLIPSE_LONG * @parameter_type int * @parameter_value 800 *************** *** 74,83 **** * @parameter_type int * @parameter_value 5000 ! * @parameter_description Longitudinal safety distance at MAX_VELOCITY speed * * @parameter_name ELLIPSE_LAT * @parameter_type int * @parameter_value 1000 ! * @parameter_description Lateral safety distance at MAX_VELOCITY speed * * @parameter_name ZERO_VEL_SECURITY_MARGIN --- 74,83 ---- * @parameter_type int * @parameter_value 5000 ! * @parameter_description Longitudinal safety distance at TYPICAL_AVOIDANCE_VELOCITY speed * * @parameter_name ELLIPSE_LAT * @parameter_type int * @parameter_value 1000 ! * @parameter_description Lateral safety distance at TYPICAL_AVOIDANCE_VELOCITY speed * * @parameter_name ZERO_VEL_SECURITY_MARGIN *************** *** 106,109 **** --- 106,114 ---- * @parameter_description Maximal rotational velocity to apply when avoiding * + * @parameter_name NB_AVOIDANCE + * @parameter_type int + * @parameter_value 5 + * @parameter_description Minimal numbers of iteration to apply avoidance + * END*/ *************** *** 127,131 **** // Parameters ! unsigned int m_maxVelocity; unsigned int m_minEllipseLong; unsigned int m_ellipseLong; --- 132,136 ---- // Parameters ! unsigned int m_typVelocity; unsigned int m_minEllipseLong; unsigned int m_ellipseLong; *************** *** 136,139 **** --- 141,145 ---- double m_rotVelSecurityMargin; int m_rotVel; + int m_nbAvoidance; // Utility class *************** *** 154,159 **** // Parameters ! ! m_maxVelocity = dereference_cast<int>(parameters.get("MAX_VELOCITY")); m_minEllipseLong = dereference_cast<int>(parameters.get("MIN_ELLIPSE_LONG")); m_ellipseLong = dereference_cast<int>(parameters.get("ELLIPSE_LONG")); --- 160,164 ---- // Parameters ! m_typVelocity = dereference_cast<int>(parameters.get("TYPICAL_AVOIDANCE_VELOCITY")); m_minEllipseLong = dereference_cast<int>(parameters.get("MIN_ELLIPSE_LONG")); m_ellipseLong = dereference_cast<int>(parameters.get("ELLIPSE_LONG")); *************** *** 164,172 **** m_rotVelSecurityMargin = static_cast<double>(dereference_cast<float>(parameters.get("ROT_VEL_SECURITY_MARGIN"))); m_rotVel = dereference_cast<int>(parameters.get("ROT_VEL")); // Utility class initialization try { ! avoidUtil = new AvoidUtil(m_maxVelocity, m_minEllipseLong, m_ellipseLong, --- 169,178 ---- m_rotVelSecurityMargin = static_cast<double>(dereference_cast<float>(parameters.get("ROT_VEL_SECURITY_MARGIN"))); m_rotVel = dereference_cast<int>(parameters.get("ROT_VEL")); + m_nbAvoidance = dereference_cast<int>(parameters.get("NB_AVOIDANCE")); // Utility class initialization try { ! avoidUtil = new AvoidUtil(m_typVelocity, m_minEllipseLong, m_ellipseLong, *************** *** 196,199 **** --- 202,211 ---- ObjectRef velInRef = getInput(velocityInID,count); + // Memory to prevent oscillation (avoiding / not avoiding) + static int memory = m_nbAvoidance; + static bool avoidActivated = false; + static unsigned int lastPos = 0; + static double lastMargin = -1; + //std::cout << "range, vel -> " << rangeValueRef->isNil() << "," << velInRef->isNil() << std::endl; if((!rangeValueRef->isNil()) && (!velInRef->isNil())){ *************** *** 221,239 **** double margin = -1; bool avoid = avoidUtil->isAvoidanceNeeded(rangeValue, velIn, pos, margin); ! std::cout << "avoid -> " << avoid << std::endl; ! std::cout << "Pos -> " << pos << std::endl; ! std::cout << "Margin -> " << margin << std::endl; ! if(!avoid) { ! // No avoidance needed ! (*outputs[velocityOutID].buffer)[count] = nilObject; ! (*outputs[rotationID].buffer)[count] = nilObject; } else { int velOut = avoidUtil->evalAvoidingLinVel(velIn, margin); int rotOut = avoidUtil->evalAvoidingRotVel(velIn, pos, margin); (*outputs[velocityOutID].buffer)[count] = ObjectRef(Int::alloc(velOut)); ! (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(rotOut));; } } --- 233,290 ---- double margin = -1; bool avoid = avoidUtil->isAvoidanceNeeded(rangeValue, velIn, pos, margin); ! ! // Memory managment ! if(avoid) { ! if(!avoidActivated) ! { ! avoidActivated = true; ! memory = m_nbAvoidance; ! } ! lastPos = pos; ! lastMargin = margin; } else { + if(avoidActivated && memory != 0) + { + pos = lastPos; + margin = lastMargin; + avoid = true; + } + else + { + avoidActivated = false; + } + } + /* + std::cout << "avoid -> " << avoid << std::endl; + std::cout << "avoidActivated -> " << avoidActivated << std::endl; + std::cout << "memory -> " << memory << std::endl; + std::cout << "velIn -> " << velIn << std::endl; + std::cout << "Pos -> " << pos << std::endl; + std::cout << "Margin -> " << margin << std::endl; + */ + if(avoid) + { + --memory; + if(memory == 0) + { + avoidActivated = false; + } + + // Compute the behavioral response int velOut = avoidUtil->evalAvoidingLinVel(velIn, margin); int rotOut = avoidUtil->evalAvoidingRotVel(velIn, pos, margin); (*outputs[velocityOutID].buffer)[count] = ObjectRef(Int::alloc(velOut)); ! (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(rotOut)); ! std::cout << "velOut -> " << velOut << std::endl; ! } ! else ! { ! // No avoidance needed ! (*outputs[velocityOutID].buffer)[count] = nilObject; ! (*outputs[rotationID].buffer)[count] = nilObject; ! std::cout << "velOut -> nilObject" << std::endl; } } Index: GotoPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/GotoPat.cc,v retrieving revision 1.5 retrieving revision 1.6 diff -C2 -d -r1.5 -r1.6 *** GotoPat.cc 5 Jul 2005 23:04:29 -0000 1.5 --- GotoPat.cc 13 Dec 2005 14:47:17 -0000 1.6 *************** *** 169,173 **** yPosID = addInput("Y_POS"); - //outputs rotationID = addOutput("ROTATION"); --- 169,172 ---- *************** *** 231,241 **** double heading = (double)(dereference_cast<int>(headingValue)); ! /*std::cout << "intermediate: " << intermediate << std::endl; std::cout << "x: " << x << std::endl; std::cout << "y: " << y << std::endl; std::cout << "xpos: " << xpos << std::endl; std::cout << "ypos: " << ypos << std::endl; ! std::cout << "heading: " << heading << std::endl;*/ ! // Distance to the desired position --- 230,239 ---- double heading = (double)(dereference_cast<int>(headingValue)); ! std::cout << "intermediate: " << intermediate << std::endl; std::cout << "x: " << x << std::endl; std::cout << "y: " << y << std::endl; std::cout << "xpos: " << xpos << std::endl; std::cout << "ypos: " << ypos << std::endl; ! std::cout << "heading: " << heading << std::endl; // Distance to the desired position Index: AvoidUtil.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidUtil.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** AvoidUtil.cc 12 Dec 2005 18:27:49 -0000 1.1 --- AvoidUtil.cc 13 Dec 2005 14:47:17 -0000 1.2 *************** *** 28,32 **** } ! AvoidUtil::AvoidUtil(unsigned int maxVelocity, unsigned int minEllipseLong, unsigned int ellipseLong, --- 28,32 ---- } ! AvoidUtil::AvoidUtil(unsigned int typVelocity, unsigned int minEllipseLong, unsigned int ellipseLong, *************** *** 37,41 **** double rotVelSecurityMargin, int rotVel) : ! m_maxVelocity(maxVelocity), m_minEllipseLong(minEllipseLong), m_ellipseLong(ellipseLong), --- 37,41 ---- double rotVelSecurityMargin, int rotVel) : ! m_typVelocity(typVelocity), m_minEllipseLong(minEllipseLong), m_ellipseLong(ellipseLong), *************** *** 52,56 **** // the longitudinal value must be greater than the lateral value // The minimal ellipse longitudinal proximity threshold must be smaller than the max speed longitudinal proximity threshold ! if((m_maxVelocity == 0) or (static_cast<int>(m_reverseVel)>m_maxVelocity) or (m_ellipseLong == 0) or (m_ellipseLat == 0) or (m_ellipseLat > m_ellipseLong) or (m_minEllipseLong > m_ellipseLong) or (rotVel ==0)) { throw AvoidUtilInvalidConstructorArguments(); --- 52,56 ---- // the longitudinal value must be greater than the lateral value // The minimal ellipse longitudinal proximity threshold must be smaller than the max speed longitudinal proximity threshold ! if((m_typVelocity == 0) or (static_cast<unsigned int>(m_reverseVel)>m_typVelocity) or (m_ellipseLong == 0) or (m_ellipseLat == 0) or (m_ellipseLat > m_ellipseLong) or (m_minEllipseLong > m_ellipseLong) or (rotVel ==0)) { throw AvoidUtilInvalidConstructorArguments(); *************** *** 72,81 **** // Evaluate the slope of the speed to avoiding distance threshold linear relation ! m_slope = static_cast<float>(m_ellipseLong) / m_maxVelocity; ! std::cout << "m_slope -> " << m_slope << std::endl; - // Evaluate the linear approximation constant for the velocity managment m_reverseVel = abs(reverseVel); ! m_reverseVel *= -1; // Force to be negative m_reverseSlope = (0-m_reverseVel) / (m_reverseVelSecurityMargin); m_reverseB = m_reverseVel; --- 72,83 ---- // Evaluate the slope of the speed to avoiding distance threshold linear relation ! m_slope = static_cast<float>(m_ellipseLong) / m_typVelocity; ! ! // Evaluate the linear approximation constant for the ! m_velSlope = (m_typVelocity-0) / (1-m_zeroVelSecurityMargin); ! m_velB = m_typVelocity - m_velSlope * 1; m_reverseVel = abs(reverseVel); ! m_reverseVel *= -1; // Force to be negative m_reverseSlope = (0-m_reverseVel) / (m_reverseVelSecurityMargin); m_reverseB = m_reverseVel; *************** *** 85,90 **** m_slopeRot = (0-m_rotVel) / (1-m_rotVelSecurityMargin); m_rotB = -1*m_slopeRot; - std::cout << "m_slopeRot -> " << m_slopeRot << std::endl; - std::cout << "m_rotB -> " << m_rotB << std::endl; } --- 87,90 ---- *************** *** 160,164 **** // Fill the belt with the avoiding distance ! unsigned int min=0; for(unsigned int i = 0; i<beltSize; ++i) { --- 160,164 ---- // Fill the belt with the avoiding distance ! int min=0; for(unsigned int i = 0; i<beltSize; ++i) { *************** *** 433,439 **** { // Evaluate linear approximation variable ! double m = velocity / (1-m_zeroVelSecurityMargin); ! double b = velocity - m * 1; ! vel = static_cast<int>(m*margin+b); } return vel; --- 433,437 ---- { // Evaluate linear approximation variable ! vel = static_cast<int>(m_velSlope * margin + m_velB); } return vel; |
|
From: Fernyqc <fe...@us...> - 2005-12-12 18:33:48
|
Update of /cvsroot/robotflow/RobotFlow In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv22112 Modified Files: configure.in Makefile.am Log Message: FCD project removed of the build system Index: Makefile.am =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Makefile.am,v retrieving revision 1.25 retrieving revision 1.26 diff -C2 -d -r1.25 -r1.26 *** Makefile.am 12 Dec 2005 18:25:51 -0000 1.25 --- Makefile.am 12 Dec 2005 18:33:39 -0000 1.26 *************** *** 16,20 **** EXTRA_DIST = RobotFlow.spec RobotFlow.spec.in ! SUBDIRS = Pioneer2 Generic Behaviors Audio Vision Control Devices Probes FSM Sensors ut FCD $(MARIE_DIR) $(OPENCV_DIR) $(PLAYER_DIR) # create the pkg-config entries --- 16,20 ---- EXTRA_DIST = RobotFlow.spec RobotFlow.spec.in ! SUBDIRS = Pioneer2 Generic Behaviors Audio Vision Control Devices Probes FSM Sensors ut $(MARIE_DIR) $(OPENCV_DIR) $(PLAYER_DIR) # create the pkg-config entries Index: configure.in =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/configure.in,v retrieving revision 1.57 retrieving revision 1.58 diff -C2 -d -r1.57 -r1.58 *** configure.in 12 Dec 2005 18:25:25 -0000 1.57 --- configure.in 12 Dec 2005 18:33:39 -0000 1.58 *************** *** 327,334 **** ut/include/Makefile \ ut/src/Makefile \ ! FCD/Makefile \ ! FCD/include/Makefile \ ! FCD/src/Makefile \ ! RobotFlow.spec \ RobotFlow.pc) --- 327,331 ---- ut/include/Makefile \ ut/src/Makefile \ ! RobotFlow.spec \ RobotFlow.pc) |
|
From: Fernyqc <fe...@us...> - 2005-12-12 18:28:57
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv21067/Behaviors/include Added Files: AvoidUtil.h Log Message: Header file use in EllipticalAvoid --- NEW FILE: AvoidUtil.h --- #ifndef AVOID_UTIL_H #define AVOID_UTIL_H #include <vector> #include <exception> // Exception classes class AvoidUtilInvalidConstructorArguments: public std::exception { virtual const char* what() const throw(); }; class AvoidUtilIllegalAvoidingThreshold: public std::exception { virtual const char* what() const throw(); }; class AvoidUtilFindSmallestMarginException: public std::exception { virtual const char* what() const throw(); }; class AvoidUtilBeltPositionOutOfRange: public std::exception { virtual const char* what() const throw(); }; class AvoidUtilMarginException: public std::exception { virtual const char* what() const throw(); }; class AvoidUtil { public: AvoidUtil(unsigned int maxVelocity, unsigned int minEllipseLong, unsigned int ellipseLong, unsigned int ellipseLat, double zeroVelSecurityMargin, double reverseVelSecurityMargin, int reverseVel, double rotVelSecurityMargin, int rotVel); ~AvoidUtil(); unsigned int evalLongAvoidingThres(int velocity) const; unsigned int evalEllipticalAvoidingThres(unsigned int theta, unsigned int distanceThres) const; bool createEllipticalAvoidingThresBelt(unsigned int beltSize, unsigned int distanceThres); const std::vector<int>& getEllipticalAvoidingThresBelt(); bool createProximityMarginBelt(std::vector<int> rangeBelt); const std::vector<double>& getProximityMarginBelt(); void findSmallestProximityMarginAndPosition(unsigned int lPos, unsigned int hPos, double& margin, unsigned int& pos); bool isAvoidanceNeeded(std::vector<int> rangeBelt, int velocity, unsigned int &pos, double &margin); int evalRotationalDirection(int velocity, unsigned int position); //unsigned int evalEllipticalDirectionChangeThres(unsigned int theta) const; // bool createEllipticalDirectionChangeThresBelt(unsigned int beltSize); //const std::vector<unsigned int>& getEllipticalDirectionChangeThresBelt(); //bool isDirectionChangeNeeded(unsigned int position, unsigned int rangeValue); int evalAvoidingLinVel(int velocity, double margin); int evalAvoidingRotVel(int velocity, unsigned int pos, double margin); private: // Function unsigned int evalEllipticalThres(unsigned int theta, unsigned int distanceThres, unsigned int ellipseLong, unsigned int ellipseLat) const; // Constructor arguments unsigned int m_maxVelocity; unsigned int m_minEllipseLong; unsigned int m_ellipseLong; unsigned int m_ellipseLat; double m_zeroVelSecurityMargin; double m_reverseVelSecurityMargin; int m_reverseVel; double m_rotVelSecurityMargin; int m_rotVel; // Variables double m_slope; double m_reverseSlope; double m_reverseB; double m_slopeRot; double m_rotB; std::vector<int> m_ellipseBelt; std::vector<double> m_proximityMarginBelt; //std::vector<int> m_directionChangeBelt; }; #endif |
|
From: Fernyqc <fe...@us...> - 2005-12-12 18:27:58
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv20399/Behaviors/src Modified Files: AvoidPat.cc Makefile.am Added Files: AvoidUtil.cc EllipticalAvoid.cc Log Message: Reffactoring of AvoidPat to EllipticalAvoid Index: Makefile.am =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/Makefile.am,v retrieving revision 1.20 retrieving revision 1.21 diff -C2 -d -r1.20 -r1.21 *** Makefile.am 27 Oct 2005 18:04:25 -0000 1.20 --- Makefile.am 12 Dec 2005 18:27:49 -0000 1.21 *************** *** 35,39 **** SafeVelocityPat.cc \ AvoidPat.cc \ ! BumperStall.cc libBehaviors_la_LDFLAGS = -release $(LT_RELEASE) --- 35,41 ---- SafeVelocityPat.cc \ AvoidPat.cc \ ! EllipticalAvoid.cc \ ! AvoidUtil.cc \ ! BumperStall.cc libBehaviors_la_LDFLAGS = -release $(LT_RELEASE) --- NEW FILE: AvoidUtil.cc --- #include "AvoidUtil.h" #include <cmath> #include <iostream> const char* AvoidUtilInvalidConstructorArguments::what() const throw() { return "illegal constructor arguments"; } const char* AvoidUtilIllegalAvoidingThreshold::what() const throw() { return "elliptical proximity safety must have a non zero longitudinal value"; } const char* AvoidUtilFindSmallestMarginException::what() const throw() { return "lPos > hPos or hPos is out of range"; } const char* AvoidUtilBeltPositionOutOfRange::what() const throw() { return "Belt position out of range"; } const char* AvoidUtilMarginException::what() const throw() { return "Margin must be between 0 and 1"; } AvoidUtil::AvoidUtil(unsigned int maxVelocity, unsigned int minEllipseLong, unsigned int ellipseLong, unsigned int ellipseLat, double zeroVelSecurityMargin, double reverseVelSecurityMargin, int reverseVel, double rotVelSecurityMargin, int rotVel) : m_maxVelocity(maxVelocity), m_minEllipseLong(minEllipseLong), m_ellipseLong(ellipseLong), m_ellipseLat(ellipseLat), m_zeroVelSecurityMargin(zeroVelSecurityMargin), m_reverseVelSecurityMargin(reverseVelSecurityMargin), m_reverseVel(reverseVel), m_rotVelSecurityMargin(rotVelSecurityMargin), m_rotVel(rotVel) { // The velocity must not be o // The max veloity has to be greater or equal than the reverse vel // The longitudinal value of the ellipse must be greater than 0 and // the longitudinal value must be greater than the lateral value // The minimal ellipse longitudinal proximity threshold must be smaller than the max speed longitudinal proximity threshold if((m_maxVelocity == 0) or (static_cast<int>(m_reverseVel)>m_maxVelocity) or (m_ellipseLong == 0) or (m_ellipseLat == 0) or (m_ellipseLat > m_ellipseLong) or (m_minEllipseLong > m_ellipseLong) or (rotVel ==0)) { throw AvoidUtilInvalidConstructorArguments(); } // The margin must be between 0 and 1 if((m_zeroVelSecurityMargin<0) or (m_zeroVelSecurityMargin>1) or (m_reverseVelSecurityMargin<0) or (m_reverseVelSecurityMargin>1) or (m_rotVelSecurityMargin<0) or (m_rotVelSecurityMargin>1)) { throw AvoidUtilInvalidConstructorArguments(); } // The margin must respect a specific order if(m_zeroVelSecurityMargin<m_reverseVelSecurityMargin) { throw AvoidUtilInvalidConstructorArguments(); } // Evaluate the slope of the speed to avoiding distance threshold linear relation m_slope = static_cast<float>(m_ellipseLong) / m_maxVelocity; std::cout << "m_slope -> " << m_slope << std::endl; // Evaluate the linear approximation constant for the velocity managment m_reverseVel = abs(reverseVel); m_reverseVel *= -1; // Force to be negative m_reverseSlope = (0-m_reverseVel) / (m_reverseVelSecurityMargin); m_reverseB = m_reverseVel; // Evaluation the linear approximation constant for the rotation managment m_rotVel = abs(m_rotVel); m_slopeRot = (0-m_rotVel) / (1-m_rotVelSecurityMargin); m_rotB = -1*m_slopeRot; std::cout << "m_slopeRot -> " << m_slopeRot << std::endl; std::cout << "m_rotB -> " << m_rotB << std::endl; } AvoidUtil::~AvoidUtil() { } unsigned int AvoidUtil::evalLongAvoidingThres(int velocity) const { // Determine the avoiding distance threshold double distance = m_slope*std::abs(static_cast<double>(velocity)); if(distance<m_minEllipseLong) { distance = m_minEllipseLong; } // Prevent exceptions on the construction of the safety belt if(distance == 0) { distance = 1; } return static_cast<unsigned int>(distance); } unsigned int AvoidUtil::evalEllipticalThres(unsigned int theta, unsigned int distanceThres, unsigned int ellipseLong, unsigned int ellipseLat) const { // Exception if the current ellipse has a longitudinal dimension equal to 0 if(distanceThres == 0) { throw AvoidUtilIllegalAvoidingThreshold(); } else { // Express theta in radian double thetaRad = theta * M_PI / 180; // Evaluate the ratio on the reference ellipse dimension double dimRatio = static_cast<double>(distanceThres) / ellipseLong; // Evaluate the dimension of the actual proximity ellipse double a = ellipseLat * dimRatio; double b = distanceThres; // Partial calculation (polar ellipse formula) double ab = a*b; double aa = a*a; double bb = b*b; double coscos = cos(thetaRad) * cos(thetaRad); double sinsin = sin(thetaRad) * sin(thetaRad); return static_cast<unsigned int>((ab) / sqrt(bb*coscos + aa*sinsin)); } } unsigned int AvoidUtil::evalEllipticalAvoidingThres(unsigned int theta, unsigned int distanceThres) const { return evalEllipticalThres(theta, distanceThres, m_ellipseLong, m_ellipseLat); } bool AvoidUtil::createEllipticalAvoidingThresBelt(unsigned int beltSize, unsigned int distanceThres) { if((beltSize == 0) or (beltSize > 360) or (distanceThres == 0)) { // Beltsize must be between 1 and 360 degree and distanceThres <> 0 return false; } else{ // Determine the angular increment for the belt unsigned int angularIncrement = 360 / beltSize; // Resize the std::Vector m_ellipseBelt.resize(beltSize); // Fill the belt with the avoiding distance unsigned int min=0; for(unsigned int i = 0; i<beltSize; ++i) { m_ellipseBelt[i] = evalEllipticalAvoidingThres(i*angularIncrement, distanceThres); // Check if the minimal safety protection is respected if(m_minEllipseLong != 0) { min = evalEllipticalAvoidingThres(i*angularIncrement, m_minEllipseLong); if(m_ellipseBelt[i] < min) { m_ellipseBelt[i] = min; } } } return true; } } const std::vector<int>& AvoidUtil::getEllipticalAvoidingThresBelt() { return m_ellipseBelt; } /*bool AvoidUtil::createEllipticalDirectionChangeThresBelt(unsigned int beltSize) { if((beltSize == 0) or (beltSize > 360)) { // Beltsize must be between 1 and 360 degree and distanceThres <> 0 return false; } else{ // Determine the angular increment for the belt unsigned int angularIncrement = 360 / beltSize; // Resize the std::Vector m_directionChangeBelt.resize(beltSize); // Fill the belt with the avoiding distance for(unsigned int i = 0; i < beltSize; ++i) { m_directionChangeBelt[i] = evalEllipticalDirectionChangeThres(i*angularIncrement); } return true; } } const std::vector<unsigned int>& AvoidUtil::getEllipticalDirectionChangeThresBelt() { return m_directionChangeBelt; }*/ bool AvoidUtil::createProximityMarginBelt(std::vector<int> rangeBelt) { if(rangeBelt.size() != m_ellipseBelt.size()) { return false; } else { m_proximityMarginBelt.resize(m_ellipseBelt.size()); for(unsigned int i = 0; i < m_ellipseBelt.size(); ++i) { m_proximityMarginBelt[i] = (static_cast<double>(rangeBelt[i]) / m_ellipseBelt[i]); } return true; } } const std::vector<double>& AvoidUtil::getProximityMarginBelt() { return m_proximityMarginBelt; } void AvoidUtil::findSmallestProximityMarginAndPosition(unsigned int lPos, unsigned int hPos, double& margin, unsigned int& pos) { // Validate exception cases if((lPos > hPos) || (hPos >= m_proximityMarginBelt.size())) { throw AvoidUtilFindSmallestMarginException(); } else { // Find the smallest margin between lPos and hPos and memorize its position double smallestMargin=m_proximityMarginBelt[lPos]; unsigned int position=lPos; for(unsigned int i=lPos+1; i<=hPos; ++i){ if(m_proximityMarginBelt[i]<smallestMargin) { position = i; smallestMargin = m_proximityMarginBelt[i]; } } // Save results and exit margin = smallestMargin; pos = position; } } // Must be generalized for belt <> 12 segments int AvoidUtil::evalRotationalDirection(int velocity, unsigned int position) { int sign = 1; if(velocity >= 0) { // If position == 3 (straight forward) if(position == 3) { // Turn on the direction of the biggest side margin if(m_proximityMarginBelt[0] > m_proximityMarginBelt[6]) { sign = -1; } else { sign = 1; } } else if ((position == 11) || (position <=2)) { // Turning left (1) if position in [11, 0, 1, 2] sign = 1; } else { // Turning right (-1) if position in [4, 5, 6, 7, 8, 9, 10] sign = -1; } } else { // If position == 9 (straight forward) if(position == 9) { // Turn on the direction of the biggest side margin if(m_proximityMarginBelt[0] <= m_proximityMarginBelt[6]) { sign = 1; } else { sign = -1; } } else if ((position >= 5) && (position <=8)) { // Turning left (1) if position in [5, 6, 7, 8] sign = -1; } else { // Turning right (-1) if position in [0, 1, 2, 3, 4, 10, 11] sign = 1; } } return sign; } bool AvoidUtil::isAvoidanceNeeded(std::vector<int> rangeBelt, int velocity, unsigned int &pos, double &margin) { double minMargin1; unsigned int minMarginPos1; double minMargin2; unsigned int minMarginPos2; // Robot is going forward or backward? if(velocity == 0) { // Find the smallest proximity margin and its position findSmallestProximityMarginAndPosition(0, 11, minMargin1, minMarginPos1); if(minMargin1 < 1) { margin = minMargin1; pos = minMarginPos1; return true; } else { return false; } } if(velocity > 0) { // Forward // Find the smallest proximity margin and its position findSmallestProximityMarginAndPosition(0, 7, minMargin1, minMarginPos1); if(m_proximityMarginBelt[11] < minMargin1) { minMargin1 = m_proximityMarginBelt[11]; minMarginPos1 = 11; } // If any proximity margin is violated if(minMargin1 < 1) { pos = minMarginPos1; margin = minMargin1; return true; } else { return false; } } else { // Backward // Find the smallest proximity margin and its position findSmallestProximityMarginAndPosition(5, 11, minMargin1, minMarginPos1); findSmallestProximityMarginAndPosition(0, 1, minMargin2, minMarginPos2); if(minMargin1 > minMargin2) { minMargin1 = minMargin2; minMarginPos1 = minMarginPos2; } // If any proximity margin is violated if(minMargin1 < 1) { pos = minMarginPos1; margin = minMargin1; return true; } else { return false; } } return false; } /* bool AvoidUtil::isDirectionChangeNeeded(unsigned int position, unsigned int rangeValue) { if(position >= m_directionChangeBelt.size()) { throw AvoidUtilBeltPositionOutOfRange(); } else { return (rangeValue < m_directionChangeBelt[position]); } }*/ int AvoidUtil::evalAvoidingLinVel(int velocity, double margin) { // Margin [0..1] if((margin < 0) or (margin > 1)) { throw AvoidUtilMarginException(); } // Deal according of the violation of the different security margin int vel=0; if(margin <= m_reverseVelSecurityMargin) { //Reverse velocity vel = static_cast<int>(m_reverseSlope * margin + m_reverseB); if(velocity<0) { vel *= -1; } } else if(margin <= m_zeroVelSecurityMargin) { // Dead zone vel = 0; } else { // Evaluate linear approximation variable double m = velocity / (1-m_zeroVelSecurityMargin); double b = velocity - m * 1; vel = static_cast<int>(m*margin+b); } return vel; } int AvoidUtil::evalAvoidingRotVel(int velocity, unsigned int pos, double margin) { // Margin [0..1] if((margin < 0) or (margin > 1)) { throw AvoidUtilMarginException(); } // Evaluation de rotational velocity according to the margin int rot=0; // Determine the magnitude if(margin <= m_rotVelSecurityMargin) { rot = m_rotVel; } else { rot = static_cast<int>(m_slopeRot * margin + m_rotB); } // Determine the sign of the rotation int sign = evalRotationalDirection(velocity, pos); // Going reverse...change the sign if(margin <= m_reverseVelSecurityMargin) { sign *= -1; } return sign * rot; } --- NEW FILE: EllipticalAvoid.cc --- /* Copyright (C) 2005 Patrick Frenette (pat...@us...) 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 _ELLIPTICAL_AVOID_CC_ #define _ELLIPTICAL_AVOID_CC_ #include "Behavior.h" #include "AvoidUtil.h" #include <exception> using namespace std; using namespace FD; namespace RobotFlow { class EllipticalAvoid; DECLARE_NODE(EllipticalAvoid) REGISTER_BEHAVIOR(EllipticalAvoid) /*Node * @name EllipticalAvoid * @category RobotFlow:Behaviors * @description No description available * * @input_name ACTIVATED * @input_type bool * @input_description Behavior activation * * @input_name RANGE_BELT * @input_type Vector<unsigned int> * @input_description Range mesure around the robot * * @input_name CURRENT_VELOCITY * @input_type int * @input_description Current velocity of the robot * * @output_name VELOCITY * @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 600 * @parameter_description Robot maximal forward velocity * * @parameter_name MIN_ELLIPSE_LONG * @parameter_type int * @parameter_value 800 * @parameter_description Longitudinal safey distance regardless of the speed * * @parameter_name ELLIPSE_LONG * @parameter_type int * @parameter_value 5000 * @parameter_description Longitudinal safety distance at MAX_VELOCITY speed * * @parameter_name ELLIPSE_LAT * @parameter_type int * @parameter_value 1000 * @parameter_description Lateral safety distance at MAX_VELOCITY speed * * @parameter_name ZERO_VEL_SECURITY_MARGIN * @parameter_type float * @parameter_value 0.5 * @parameter_description Security margin to apply a zero velocity value [0..1] * * @parameter_name REVERSE_VEL_SECURITY_MARGIN * @parameter_type float * @parameter_value 0.2 * @parameter_description Security margin to reverse the direction of motion [0..1] * * @parameter_name REVERSE_VEL * @parameter_type int * @parameter_value 100 * @parameter_description Reverse velocity to apply when the margin of security is below REVERSE_VEL_SECURITY_MARGIN * * @parameter_name ROT_VEL_SECURITY_MARGIN * @parameter_type float * @parameter_value 0.5 * @parameter_description Security margin to apply the ROT_VEL value * * @parameter_name ROT_VEL * @parameter_type int * @parameter_value 20 * @parameter_description Maximal rotational velocity to apply when avoiding * END*/ class EllipticalAvoidException: public std::exception { virtual const char* what() const throw() { return "General exception in behaviors EllipticalAvoid"; } }; class EllipticalAvoid : public Behavior { //inputs int rangeID; int velocityInID; //outputs int velocityOutID; int rotationID; // Parameters unsigned int m_maxVelocity; unsigned int m_minEllipseLong; unsigned int m_ellipseLong; unsigned int m_ellipseLat; double m_zeroVelSecurityMargin; double m_reverseVelSecurityMargin; int m_reverseVel; double m_rotVelSecurityMargin; int m_rotVel; // Utility class AvoidUtil* avoidUtil; public: EllipticalAvoid(string nodeName, ParameterSet params) : Behavior(nodeName, params, "EllipticalAvoid") { //inputs rangeID = addInput("RANGE_BELT"); velocityInID = addInput("CURRENT_VELOCITY"); //outputs velocityOutID = addOutput("VELOCITY"); rotationID = addOutput("ROTATION"); // Parameters m_maxVelocity = dereference_cast<int>(parameters.get("MAX_VELOCITY")); m_minEllipseLong = dereference_cast<int>(parameters.get("MIN_ELLIPSE_LONG")); m_ellipseLong = dereference_cast<int>(parameters.get("ELLIPSE_LONG")); m_ellipseLat = dereference_cast<int>(parameters.get("ELLIPSE_LAT")); m_zeroVelSecurityMargin = static_cast<double>(dereference_cast<float>(parameters.get("ZERO_VEL_SECURITY_MARGIN"))); m_reverseVelSecurityMargin = static_cast<double>(dereference_cast<float>(parameters.get("REVERSE_VEL_SECURITY_MARGIN"))); m_reverseVel = dereference_cast<int>(parameters.get("REVERSE_VEL")); m_rotVelSecurityMargin = static_cast<double>(dereference_cast<float>(parameters.get("ROT_VEL_SECURITY_MARGIN"))); m_rotVel = dereference_cast<int>(parameters.get("ROT_VEL")); // Utility class initialization try { avoidUtil = new AvoidUtil(m_maxVelocity, m_minEllipseLong, m_ellipseLong, m_ellipseLat, m_zeroVelSecurityMargin, m_reverseVelSecurityMargin, m_reverseVel, m_rotVelSecurityMargin, m_rotVel); } catch (AvoidUtilInvalidConstructorArguments) { throw EllipticalAvoidException(); } } ~EllipticalAvoid() { delete avoidUtil; } void calculate_behavior(int output_id, int count, Buffer &out) { //std::cout << "EllipticalAvoid : output_id -> " << output_id << std::endl; //std::cout << "EllipticalAvoid -> getInput1" << std::endl; ObjectRef rangeValueRef = getInput(rangeID,count); //std::cout << "EllipticalAvoid -> getInput2s" << std::endl; ObjectRef velInRef = getInput(velocityInID,count); //std::cout << "range, vel -> " << rangeValueRef->isNil() << "," << velInRef->isNil() << std::endl; if((!rangeValueRef->isNil()) && (!velInRef->isNil())){ // Cast the range belt Vector<int> &rangeValue = object_cast<Vector<int> >(getInput(rangeID,count)); // Get the current velocity of the robot int velIn = dereference_cast<int>(velInRef); // According to the actual velocity, create the safety belt unsigned int distThres = avoidUtil->evalLongAvoidingThres(velIn); if(!avoidUtil->createEllipticalAvoidingThresBelt(rangeValue.size(), distThres)) { throw EllipticalAvoidException(); } // Compare the current range belt to the safety belt if(!avoidUtil->createProximityMarginBelt(rangeValue)) { throw EllipticalAvoidException(); } // Avoid (if needed) unsigned int pos = 1000; double margin = -1; bool avoid = avoidUtil->isAvoidanceNeeded(rangeValue, velIn, pos, margin); std::cout << "avoid -> " << avoid << std::endl; std::cout << "Pos -> " << pos << std::endl; std::cout << "Margin -> " << margin << std::endl; if(!avoid) { // No avoidance needed (*outputs[velocityOutID].buffer)[count] = nilObject; (*outputs[rotationID].buffer)[count] = nilObject; } else { int velOut = avoidUtil->evalAvoidingLinVel(velIn, margin); int rotOut = avoidUtil->evalAvoidingRotVel(velIn, pos, margin); (*outputs[velocityOutID].buffer)[count] = ObjectRef(Int::alloc(velOut)); (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(rotOut));; } } else{ (*outputs[velocityOutID].buffer)[count] = nilObject; (*outputs[rotationID].buffer)[count] = nilObject; std::cout << "inputs -> nilObject" << std::endl; } }//calculate_behavior }; }//namespace RobotFlow #endif Index: AvoidPat.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidPat.cc,v retrieving revision 1.14 retrieving revision 1.15 diff -C2 -d -r1.14 -r1.15 *** AvoidPat.cc 7 Jul 2005 04:56:45 -0000 1.14 --- AvoidPat.cc 12 Dec 2005 18:27:49 -0000 1.15 *************** *** 44,50 **** * @input_description Range mesure around the robot * ! * @input_name ACTUAL_VELOCITY * @input_type int ! * @input_description Actual velocity of the robot * * @output_name VELOCITY --- 44,50 ---- * @input_description Range mesure around the robot * ! * @input_name CURRENT_VELOCITY * @input_type int ! * @input_description Current velocity of the robot * * @output_name VELOCITY *************** *** 62,81 **** * @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). * * @parameter_name MIN_ROTATION * @parameter_type int * @parameter_value 5 ! * @parameter_description Minimum rotation to get something going (degree/s). * * @parameter_name MIN_ROTATION_GAIN --- 62,81 ---- * @parameter_type int * @parameter_value 200 ! * @parameter_description Maximum avoiding velocity allowed (mm/s). * * @parameter_name MIN_AVOID_VELOCITY * @parameter_type int ! * @parameter_value 0 ! * @parameter_description Minimum avoiding velocity to help the rotation (mm/s). * * @parameter_name MAX_ROTATION * @parameter_type int * @parameter_value 15 ! * @parameter_description Maximum avoiding rotational speed allowed (degree/s). * * @parameter_name MIN_ROTATION * @parameter_type int * @parameter_value 5 ! * @parameter_description Minimum avoiding rotational speed (to get something going) (degree/s). * * @parameter_name MIN_ROTATION_GAIN *************** *** 87,91 **** * @parameter_type int * @parameter_value 300 ! * @parameter_description Maximum distance of the protection (mm) * * @parameter_name MAX_VEL_CORRECTION --- 87,91 ---- * @parameter_type int * @parameter_value 300 ! * @parameter_description Velocity to consider MAX_DISTANCE to start avoiding (mm/s) * * @parameter_name MAX_VEL_CORRECTION *************** *** 97,106 **** * @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 MARGIN_TOL --- 97,106 ---- * @parameter_type int * @parameter_value 600 ! * @parameter_description Avoiding will occur if an obstacle is detected below MAX_DISTANCE from the longitidunal axis of the robot at MAX_ROBOT_VELOCITY (mm) * * @parameter_name MIN_DISTANCE * @parameter_type int * @parameter_value 150 ! * @parameter_description Below this distance, avoiding will occur regardless of the current velocity (mm) * * @parameter_name MARGIN_TOL *************** *** 127,136 **** * @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*/ --- 127,136 ---- * @parameter_type int * @parameter_value 5 ! * @parameter_description Rotational speed to apply when going backward (mm/s) * * @parameter_name BACKWARD_ITERATION * @parameter_type int * @parameter_value 5 ! * @parameter_description Number of iteration to apply the backward outputs * END*/ *************** *** 166,170 **** // Slope of the line used to determine security distance float m_dist_slope; - float m_dist_b; float m_vel_slope; --- 166,169 ---- *************** *** 256,260 **** //inputs rangeID = addInput("RANGE_BELT"); ! velocityInID = addInput("ACTUAL_VELOCITY"); //outputs --- 255,259 ---- //inputs rangeID = addInput("RANGE_BELT"); ! velocityInID = addInput("CURRENT_VELOCITY"); //outputs *************** *** 280,286 **** 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; } void calculate_behavior(int output_id, int count, Buffer &out) { --- 279,285 ---- if(m_backward_iter<1) m_backward_iter = 1; ! m_dist_slope = (1.0 * m_max_distance) / m_max_robot_vel; } + void calculate_behavior(int output_id, int count, Buffer &out) { *************** *** 313,317 **** } else{ ! // Get the actual velocity of the robot int velIn = m_max_robot_vel; if(!velInRef->isNil()){ --- 312,316 ---- } else{ ! // Get the current velocity of the robot int velIn = m_max_robot_vel; if(!velInRef->isNil()){ *************** *** 321,329 **** } ! // 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 --- 320,327 ---- } ! // Determine the avoiding distance threshold ! float securityDist = m_dist_slope*abs(velIn); ! if(securityDist<m_min_distance) securityDist = m_min_distance; // Evaluate the security margin around the robot *************** *** 336,343 **** 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; --- 334,343 ---- 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; *************** *** 345,352 **** } ! // 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]; --- 345,352 ---- } ! // Find the smallest security margin ignoring range 5, 6 and 7 (back sensor...going forward) 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]; *************** *** 444,447 **** --- 444,449 ---- }//calculate_behavior + + }; |
|
From: Fernyqc <fe...@us...> - 2005-12-12 18:26:03
|
Update of /cvsroot/robotflow/RobotFlow In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv20006 Modified Files: Makefile.am Log Message: Support for the unit test blocks Index: Makefile.am =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Makefile.am,v retrieving revision 1.24 retrieving revision 1.25 diff -C2 -d -r1.24 -r1.25 *** Makefile.am 27 Oct 2005 19:02:59 -0000 1.24 --- Makefile.am 12 Dec 2005 18:25:51 -0000 1.25 *************** *** 16,20 **** EXTRA_DIST = RobotFlow.spec RobotFlow.spec.in ! SUBDIRS = Pioneer2 Generic Behaviors Audio Vision Control Devices Probes FSM Sensors $(MARIE_DIR) $(OPENCV_DIR) $(PLAYER_DIR) # create the pkg-config entries --- 16,20 ---- EXTRA_DIST = RobotFlow.spec RobotFlow.spec.in ! SUBDIRS = Pioneer2 Generic Behaviors Audio Vision Control Devices Probes FSM Sensors ut FCD $(MARIE_DIR) $(OPENCV_DIR) $(PLAYER_DIR) # create the pkg-config entries |
|
From: Fernyqc <fe...@us...> - 2005-12-12 18:25:34
|
Update of /cvsroot/robotflow/RobotFlow In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv19915 Modified Files: configure.in Log Message: Support for the unit test blocks Index: configure.in =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/configure.in,v retrieving revision 1.56 retrieving revision 1.57 diff -C2 -d -r1.56 -r1.57 *** configure.in 27 Oct 2005 18:02:17 -0000 1.56 --- configure.in 12 Dec 2005 18:25:25 -0000 1.57 *************** *** 324,327 **** --- 324,333 ---- Sensors/include/Makefile \ Sensors/src/Makefile \ + ut/Makefile \ + ut/include/Makefile \ + ut/src/Makefile \ + FCD/Makefile \ + FCD/include/Makefile \ + FCD/src/Makefile \ RobotFlow.spec \ RobotFlow.pc) |
|
From: Fernyqc <fe...@us...> - 2005-12-12 18:24:54
|
Update of /cvsroot/robotflow/RobotFlow/ut/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv19653/ut/src Added Files: Makefile.am UtLoad.cc UtSave.cc Log Message: Files of the unit test blocks --- NEW FILE: Makefile.am --- ## Process this file with automake to produce Makefile.in. -*-Makefile-*- # $Id: Makefile.am,v 1.1 2005/12/12 18:24:45 fernyqc Exp $ # Disable automatic dependency tracking if using other tools than gcc and gmake #AUTOMAKE_OPTIONS = no-dependencies lib_LTLIBRARIES = libRFut.la # Sources for compilation in the library libRFut_la_SOURCES = UtLoad.cc \ UtSave.cc libRFut_la_LDFLAGS = -release $(LT_RELEASE) INCLUDES = -I../include $(OVERFLOW_INCLUDE) install-data-local: echo "Installing libRFut in $(libdir)" mkdir -p $(libdir) (perl $(OVERFLOW_BIN)/info2def.pl $(libRFut_la_SOURCES) > $(prefix)/RFut.def) (rm -f $(libdir)/RFut.tlb; cd $(libdir); ln -s libRFut.so RFut.tlb) --- NEW FILE: UtLoad.cc --- /* Copyright (C) 2005 Patrick Frenette (pat...@us...) 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 */ #include "BufferedNode.h" #include <Vector.h> #include <exception> #include <string> #include <fstream> #include <iostream> using namespace std; using namespace FD; namespace RobotFlow { class UtLoad; DECLARE_NODE(UtLoad) /*Node * * @name UtLoad * @category UnitTest * @description Save input(s) to a file * * @output_name EOF * @output_type bool * @output_description Is the file empty * * @parameter_name HEADERFILE * @parameter_type string * @parameter_value * @parameter_description The filename of the header definition * * @parameter_name TESTFILE * @parameter_type string * @parameter_value * @parameter_description The filename containing the data for the test * END*/ class UtFileException: public std::exception { virtual const char* what() const throw() { return "There are problems with the file (empty or non-existent)"; } }; class UtUnsupportedDataType: public std::exception { virtual const char* what() const throw() { return "Unsupported data type"; } }; class UtOutputMismatch: public std::exception { virtual const char* what() const throw() { return "EOF output as not the index 0. Don't delete it when adding a new block"; } }; class UtLoad : public BufferedNode { private: // Output int m_eofID; // File descriptor ifstream m_file; // Type header std::vector<std::string> m_header; public: UtLoad(std::string nodeName, ParameterSet params) : BufferedNode(nodeName, params) { // --------------------- // Data type header file // --------------------- RCPtr<String> strHeader = parameters.get("HEADERFILE"); ifstream fileHeader; fileHeader.open(strHeader->val().c_str()); if( !fileHeader.is_open() ) { throw UtFileException(); } else { std::string dataType; while(fileHeader.good()) { fileHeader >> dataType; if(fileHeader.good()) { m_header.push_back(dataType); } } } fileHeader.close(); // --------------------- // Testfile managment // --------------------- RCPtr<String> strFile = parameters.get("TESTFILE"); m_file.open(strFile->val().c_str()); if( !m_file.is_open() ) { throw UtFileException(); } } int translateOutput (string outputName) { // Prevent the registration of the same output for (unsigned int i=0; i<outputNames.size(); i++) { if (outputNames[i] == outputName) { return i; } } // Get an output ID and save the one for EOF int numOutput = addOutput(outputName); if(outputName == "EOF") { m_eofID = numOutput; // Throw exception if the EOF is not the outputID 0 if(m_eofID != 0) { throw UtOutputMismatch(); } } //std::cout << "translateOutput -> " << outputName << ", " << numOutput << std::endl; return numOutput; } void calculate(int output_id, int count, Buffer &out) { // Deal with the output if(output_id == m_eofID) { // Return the EOF state (*outputs[output_id].buffer)[count] = ObjectRef(Bool::alloc(!m_file.good())); //std::cout << "calculate -> " << output_id << ", EOF -> " << !m_file.good() << std::endl; } else { // Default output value -> unsuported data type ObjectRef output = nilObject; // Always be careful for the file error (EOF for example) if(m_file.good()) { // Substract 1 to the output_id to consider the output EOF int outID = output_id - 1; // Read the data according to the data type if(m_header[outID] == "String") { std::string data; m_file >> data; if(m_file.good()) { output = ObjectRef(new String(data.c_str())); //std::cout << "utLoad -> " << output_id << ", data -> " << data << std::endl; } } else if(m_header[outID] == "Int") { int data; m_file >> data; if(m_file.good()) { output = ObjectRef(Int::alloc(data)); //std::cout << "utLoad -> " << output_id << ", data -> " << data << std::endl; } } else if(m_header[outID] == "Float") { float data; m_file >> data; if(m_file.good()) { output = ObjectRef(Float::alloc(data)); //std::cout << "utLoad -> " << output_id << ", data -> " << data << std::endl; } } else if(m_header[outID] == "Bool") { bool data; m_file >> data; if(m_file.good()) { output = ObjectRef(Bool::alloc(data)); //std::cout << "utLoad -> " << output_id << ", data -> " << data << std::endl; } } else if(m_header[outID] == "Vector<int>") { // Read the numbers of elements int nb = 0; m_file >> nb; if(m_file.good()) { // Create the vector Vector<int> *data = Vector<int>::alloc(nb); std::cout << "utLoad -> " << output_id << ", data -> " << data->size() << " "; // Read and save the value for(unsigned int i=0; i<nb; ++i) { if(m_file.good()) { m_file >> (*data)[i]; std::cout << (*data)[i] << " "; } } std::cout << std::endl; // Output the vector if(m_file.good()) { // Ouput the vector output = ObjectRef(data); } } } else { // Unsupported data type throw UtUnsupportedDataType(); } } // Output (*outputs[output_id].buffer)[count] = output; //if(output->isNil()) //{ // std::cout << "utLoad -> " << output_id << ", data -> nilObject" << std::endl; //} } } }; }//namespace RobotFlow --- NEW FILE: UtSave.cc --- /* Copyright (C) 2005 Patrick Frenette (pat...@us...) 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 */ #include "BufferedNode.h" #include <Vector.h> #include <exception> #include <string> #include <fstream> #include <iostream> using namespace std; using namespace FD; namespace RobotFlow { class UtSave; DECLARE_NODE(UtSave) /*Node * * @name UtSave * @category UnitTest * @description Save input(s) to a file * * @output_name NILOUTPUT * @output_type any * @output_description * * @parameter_name FILENAME * @parameter_type string * @parameter_value * @parameter_description The filename to save the inputs * END*/ class UtCannotOpenFile: public std::exception { virtual const char* what() const throw() { return "Cannot open the file. Look at the parameter value."; } }; class UtSave : public BufferedNode { private: //outputs int m_outputID; // File descriptor ofstream m_file; public: UtSave(std::string nodeName, ParameterSet params) : BufferedNode(nodeName, params) { m_outputID = addOutput("NILOUTPUT"); // Get the filename RCPtr<String> strFile = parameters.get("FILENAME"); // Open the stream m_file.open(strFile->val().c_str()); if( !m_file.is_open() ) { std::cout << "UtSave -> Cannot open the file. Look at the parameter value." << std::endl; throw UtCannotOpenFile(); } } int translateInput (std::string inputName) { for (unsigned int i=0; i< inputs.size(); i++) { if (inputs[i].name == inputName) { return i; } } return addInput(inputName); } void calculate(int output_id, int count, Buffer &out) { // Always output a nil object out[count] = nilObject; // For each input bool isInput = false; for (unsigned int i=0; i<inputs.size(); ++i) { // Write the input to the file ObjectRef in = getInput(i, count); if(m_file.good()) { if(!in->isNil()) { isInput = true; // Get the className std::string className(in->className()); // Print out the supported datatype if(className == "String") { RCPtr<String> params = in; m_file << params->val(); //std::cout << "input -> " << i << ", string -> " << params->val() << std::endl; } else if(className == "Int") { int inInt = dereference_cast<int>(in); m_file << inInt; //std::cout << "input -> " << i << ", int -> " << inInt << std::endl; } else if(className == "Float") { float inFloat = dereference_cast<float>(in); m_file << inFloat; //std::cout << "input -> " << i << ", float -> " << inFloat << std::endl; } else if(className == "Bool") { bool inBool = dereference_cast<bool>(in); m_file << inBool; //std::cout << "input -> " << i << ", bool -> " << inBool << std::endl; } else if(className == "Vector<int>") { Vector<int> &inVector = object_cast<Vector<int> >(in); m_file << inVector.size() << " "; //std::cout << "input -> " << i << ", vector<int> -> "; for(unsigned int i=0; i<inVector.size(); ++i) { if(m_file.good()) { m_file << inVector[i]; //std::cout << inVector[i]; if(m_file.good() && i != inVector.size()-1) { m_file << " "; //std::cout << " "; } } } //std::cout << std::endl; } else { // Unsupported type m_file << "Unsupported"; //std::cout << "input -> " << i << ", unsupported" << std::endl; } } else { m_file << "NilObject"; //std::cout << "input -> " << i << ", nilObject" << std::endl; } // Add a space except at the end of the line if((m_file.good()) && (i < inputs.size() - 1)) { m_file << " "; } } } // Newline if(m_file.good() && isInput) { m_file << std::endl; } } }; }//namespace RobotFlow |
|
From: Fernyqc <fe...@us...> - 2005-12-12 18:24:53
|
Update of /cvsroot/robotflow/RobotFlow/ut/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv19653/ut/include Added Files: Makefile.am Log Message: Files of the unit test blocks --- NEW FILE: Makefile.am --- AUTOMAKE_OPTIONS = no-dependencies include_HEADERS = noinst_HEADERS = |
|
From: Fernyqc <fe...@us...> - 2005-12-12 18:24:53
|
Update of /cvsroot/robotflow/RobotFlow/ut/n-files In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv19653/ut/n-files Added Files: header.txt in.txt README utExample.n Log Message: Files of the unit test blocks --- NEW FILE: README --- 1) Look at the utExample.n for a simple network using utLoad and utSave 2) You manually have to issue a diff command to see if the test is successful (i.e.: diff in.txt out.txt) --- NEW FILE: header.txt --- String Int Float Bool Vector<int> --- NEW FILE: utExample.n --- #!/usr/bin/env batchflow <?xml version="1.0"?> <Document> <Network type="subnet" name="MAIN"> <Node name="node_LOOP0_1" type="LOOP0" x="62.000000" y="100.000000"> <Parameter name="DOWHILE" type="bool" value="" description="No description available"/> </Node> <NetOutput name="OUTPUT" node="node_LOOP0_1" terminal="OUTPUT" object_type="any" description="No description available"/> <Note x="0" y="0" visible="0" text="Created with FlowDesigner 0.9.0"/> </Network> <Network type="iterator" name="LOOP0"> <Node name="node_NOT_1" type="NOT" x="282.000000" y="75.000000"/> <Node name="node_UtLoad_1" type="UtLoad" x="166.000000" y="121.000000"> <Parameter name="HEADERFILE" type="string" value="header.txt" description="The filename of the header definition"/> <Parameter name="TESTFILE" type="string" value="in.txt" description="The filename containing the data for the test"/> </Node> <Node name="node_NOP_1" type="NOP" x="515.000000" y="128.000000"/> <Node name="node_UtSave_1" type="UtSave" x="313.000000" y="129.000000"> <Parameter name="FILENAME" type="string" value="out.txt" description="The filename to save the inputs"/> </Node> <Link from="node_UtLoad_1" output="EOF" to="node_NOT_1" input="INPUT"/> <Link from="node_UtSave_1" output="NILOUTPUT" to="node_NOP_1" input="INPUT"/> <Link from="node_UtLoad_1" output="o1" to="node_UtSave_1" input="i1"/> <Link from="node_UtLoad_1" output="o2" to="node_UtSave_1" input="i2"/> <Link from="node_UtLoad_1" output="o3" to="node_UtSave_1" input="i3"/> <Link from="node_UtLoad_1" output="o4" to="node_UtSave_1" input="i4"/> <Link from="node_UtLoad_1" output="o5" to="node_UtSave_1" input="i5"/> <NetCondition name="CONDITION" node="node_NOT_1" terminal="OUTPUT"/> <NetOutput name="OUTPUT" node="node_NOP_1" 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> </Document> --- NEW FILE: in.txt --- Patrick 10 11 0 3 1 2 3 Roger 1 2 1 3 4 5 6 William 100 2.98 1 3 7 8 9 |
|
From: Fernyqc <fe...@us...> - 2005-12-12 18:23:20
|
Update of /cvsroot/robotflow/RobotFlow/ut In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv18820/ut Added Files: Makefile.am Log Message: Addition of the unit test block for FD --- NEW FILE: Makefile.am --- ## Process this file with automake to produce Makefile.in. -*-Makefile-*- # Disable automatic dependency tracking if using other tools than gcc and gmake #AUTOMAKE_OPTIONS = no-dependencies SUBDIRS = src include |
|
From: Fernyqc <fe...@us...> - 2005-12-12 18:22:23
|
Update of /cvsroot/robotflow/RobotFlow/ut/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv18504/ut/src Log Message: Directory /cvsroot/robotflow/RobotFlow/ut/src added to the repository |
|
From: Fernyqc <fe...@us...> - 2005-12-12 18:22:14
|
Update of /cvsroot/robotflow/RobotFlow/ut/n-files In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv18430/ut/n-files Log Message: Directory /cvsroot/robotflow/RobotFlow/ut/n-files added to the repository |
|
From: Fernyqc <fe...@us...> - 2005-12-12 18:21:35
|
Update of /cvsroot/robotflow/RobotFlow/ut/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv18259/ut/include Log Message: Directory /cvsroot/robotflow/RobotFlow/ut/include added to the repository |
|
From: Fernyqc <fe...@us...> - 2005-12-12 18:21:09
|
Update of /cvsroot/robotflow/RobotFlow/ut In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv18175/ut Log Message: Directory /cvsroot/robotflow/RobotFlow/ut added to the repository |
|
From: Fernyqc <fe...@us...> - 2005-12-12 18:20:32
|
Update of /cvsroot/robotflow/RobotFlow/Sensors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv18036/Sensors/src Modified Files: RangeFashion.cc Log Message: Refactoring to start the belt at 0 degree; Deactivate the IR and Sonar support Index: RangeFashion.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Sensors/src/RangeFashion.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** RangeFashion.cc 27 Oct 2005 18:02:18 -0000 1.1 --- RangeFashion.cc 12 Dec 2005 18:20:23 -0000 1.2 *************** *** 104,108 **** // Priority #1 // IRs ! if(!irValue->isNil()){ Vector<int> &irReading = object_cast<Vector<int> >(getInput(irID,count)); --- 104,108 ---- // Priority #1 // IRs ! /*if(!irValue->isNil()){ Vector<int> &irReading = object_cast<Vector<int> >(getInput(irID,count)); *************** *** 115,119 **** ++i; } ! } /*Vector<int>::iterator iterA; --- 115,119 ---- ++i; } ! }*/ /*Vector<int>::iterator iterA; *************** *** 126,136 **** // 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; --- 126,138 ---- // Priority #2 // Lasers ! if(!laserValue->isNil()) ! { Vector<int> &laserReading = object_cast<Vector<int> >(getInput(laserID,count)); ! int ind_vectPercept = 0; // 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; *************** *** 142,155 **** int min1=m_maxDistLaser, min2=m_maxDistLaser, min3=m_maxDistLaser; 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]; } --- 144,160 ---- int min1=m_maxDistLaser, min2=m_maxDistLaser, min3=m_maxDistLaser; 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]; } *************** *** 159,183 **** 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 = 200; 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 < m_maxDistLaser)){ ! (*vectPercept)[ind_vectPercept] = x_min - offset; ! } ! // Next position ! --ind_vectPercept; ! if(ind_vectPercept<0) ! ind_vectPercept = 11; } } --- 164,183 ---- 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 = 200; 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)[i] == -1) && (x_min < m_maxDistLaser)){ ! (*vectPercept)[i] = x_min - offset; ! } } } *************** *** 191,195 **** // Priority #3 // Sonars ! if(!sonarValue->isNil()){ Vector<int> &sonarReading = object_cast<Vector<int> >(getInput(sonarID,count)); --- 191,195 ---- // Priority #3 // Sonars ! /*if(!sonarValue->isNil()){ Vector<int> &sonarReading = object_cast<Vector<int> >(getInput(sonarID,count)); *************** *** 202,206 **** ++i; } ! } /*std::cout << "Sonar -> "; --- 202,206 ---- ++i; } ! }*/ /*std::cout << "Sonar -> "; |
|
From: Nicolas P. <npa...@us...> - 2005-11-25 00:03:31
|
Update of /cvsroot/robotflow/RobotFlow/OpenCV/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv27623/OpenCV/src Modified Files: CvCaptureFrame.cc CvHaarDetect.cc Log Message: Cleaned code and removed releaseImage that makes FlowDesigner crash. Index: CvHaarDetect.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/OpenCV/src/CvHaarDetect.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** CvHaarDetect.cc 24 Nov 2005 21:15:07 -0000 1.2 --- CvHaarDetect.cc 25 Nov 2005 00:03:23 -0000 1.3 *************** *** 160,164 **** virtual ~CvHaarDetect() { ! cerr<<"CvHaarDetect destruction"<<endl; cvReleaseImage(&m_curImage); --- 160,164 ---- virtual ~CvHaarDetect() { ! //cerr<<"CvHaarDetect destruction"<<endl; cvReleaseImage(&m_curImage); *************** *** 266,279 **** retval = true; - /*if( saveDetected ) - { - strcpy( detfilename, detname ); - strcat( detfilename, filename ); - strcpy( filename, detfilename ); - cvvSaveImage( fullname, m_curImage ); - }*/ - - //FIXME: Mem leek possible !!! - //if( det ) { cvFree( (void**)&det ); det = NULL; } } --- 266,269 ---- *************** *** 289,293 **** private: int m_imageInID; - int m_maskInID; int m_activatedInID; int m_roiOutID; --- 279,282 ---- *************** *** 306,315 **** int m_imgYCen; - int m_maxNumSkinRegions; - int m_minROIWidth; - int m_minROIHeight; - CvMemStorage *m_storage1; - CvMemStorage *m_storage2; CvHaarClassifierCascade *m_haarClassifier1; --- 295,299 ---- *************** *** 320,329 **** CvMemStorage *m_contourStorage; ! RCPtr<Image> m_outputImage; ! //RCPtr<VisualROI> m_objectROI; ! RCPtr<Vector<ObjectRef> > m_ROIVect; ! ! bool isFrontFace; ! bool isProfileFace; }; --- 304,308 ---- CvMemStorage *m_contourStorage; ! RCPtr<Vector<ObjectRef> > m_ROIVect; }; Index: CvCaptureFrame.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/OpenCV/src/CvCaptureFrame.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** CvCaptureFrame.cc 24 Nov 2005 21:15:07 -0000 1.2 --- CvCaptureFrame.cc 25 Nov 2005 00:03:23 -0000 1.3 *************** *** 119,125 **** throw new GeneralException("PIXELSIZE not yet supported",__FILE__,__LINE__); } ! ! cvReleaseImage(&m_frame); ! m_frame = NULL; } } --- 119,126 ---- throw new GeneralException("PIXELSIZE not yet supported",__FILE__,__LINE__); } ! ! //FIXME: It crached when tring to release m_frame ? ! //cvReleaseImage(&m_frame); ! //m_frame = NULL; } } |
