[Robotflow-cvs] RobotFlow/Behaviors/src/ut AvoidTest.cc,NONE,1.1 AvoidTest.h,NONE,1.1 gcc.txt,NONE,1.1 utAvoid,NONE,1.1 ut.cc,NONE,1.1

[Fernyqc <fe...@us...>]

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));
}