Modules/SelfLocator/ComboLocator.cpp

Go to the documentation of this file.

/**
 * @author <A href=mailto:c_rohde@web.de>Carsten Rohde</A>
 */

#include "Platform/GTAssert.h"
#include "Platform/SystemCall.h"
#include "Tools/FieldDimensions.h"
#include "Tools/Player.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Streams/InStreams.h"
#include "Tools/Debugging/GenericDebugData.h"
#include "ComboLocator.h"

// #define _COMPARE_GT
#define _EVOLVE

ComboLocator::ComboLocator(const SelfLocatorInterfaces& interfaces)
: SelfLocator(interfaces)
{
  locator1 = new GT2005StableSelfLocator(interfaces);
  locator2 = new GT2005SelfLocator(interfaces);
  result1 = new double[NO_OF_FRAMES];
  result2 = new double[NO_OF_FRAMES];
  frames = 0;
  finished = false;
}

ComboLocator::~ComboLocator(void)
{
  delete locator1;
  delete locator2;
  delete[] result1;
  delete[] result2;
}


void ComboLocator::execute()
{ 
  currentPose = gtCamWorldState.getRobotPose();
  if (currentPose.translation.x==0 && currentPose.translation.y==0)
    return;

  if (frames%(30*60)==0)
    OUTPUT(idText,text,frames << " frames");

  if (frames>=NO_OF_FRAMES-1 && !finished){
    OUTPUT(idText,text,"dumping differences:");
    OutTextFile stream ("/MS/diff.csv");
    stream << "stable;gt05\n"; 
    for(int i=0;i<frames;i++)
    {
      stream << result1[i] << ";" << result2[i]<< "\n";
    }
    finished = true;
  }
  locator1->execute();
  pose1 = Pose2D(robotPose);
  locator2->execute();
  pose2 = Pose2D(robotPose);
  
  dummy = pose1.translation - currentPose.translation;
  result1[frames] = dummy.abs();
  dummy = pose2.translation - currentPose.translation;
  result2[frames] = dummy.abs();
  frames++;
}

bool ComboLocator::handleMessage(InMessage& message) {
  return false;
}

---