Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/SelfLocator/ComboLocator.cpp

Go to the documentation of this file.

    00001 /**
    00002  * @author <A href=mailto:c_rohde@web.de>Carsten Rohde</A>
    00003  */
    00004
    00005 #include "Platform/GTAssert.h"
    00006 #include "Platform/SystemCall.h"
    00007 #include "Tools/FieldDimensions.h"
    00008 #include "Tools/Player.h"
    00009 #include "Tools/Debugging/Debugging.h"
    00010 #include "Tools/Math/Geometry.h"
    00011 #include "Tools/Streams/InStreams.h"
    00012 #include "Tools/Debugging/GenericDebugData.h"
    00013 #include "ComboLocator.h"
    00014
    00015 // #define _COMPARE_GT
    00016 #define _EVOLVE
    00017
    00018 ComboLocator::ComboLocator(const SelfLocatorInterfaces& interfaces)
    00019 : SelfLocator(interfaces)
    00020 {
    00021   locator1 = new GT2005StableSelfLocator(interfaces);
    00022   locator2 = new GT2005SelfLocator(interfaces);
    00023   result1 = new double[NO_OF_FRAMES];
    00024   result2 = new double[NO_OF_FRAMES];
    00025   frames = 0;
    00026   finished = false;
    00027 }
    00028
    00029 ComboLocator::~ComboLocator(void)
    00030 {
    00031   delete locator1;
    00032   delete locator2;
    00033   delete[] result1;
    00034   delete[] result2;
    00035 }
    00036
    00037
    00038 void ComboLocator::execute()
    00039 {
    00040   currentPose = gtCamWorldState.getRobotPose();
    00041   if (currentPose.translation.x==0 && currentPose.translation.y==0)
    00042     return;
    00043
    00044   if (frames%(30*60)==0)
    00045     OUTPUT(idText,text,frames << " frames");
    00046
    00047   if (frames>=NO_OF_FRAMES-1 && !finished){
    00048     OUTPUT(idText,text,"dumping differences:");
    00049     OutTextFile stream ("/MS/diff.csv");
    00050     stream << "stable;gt05\n";
    00051     for(int i=0;i<frames;i++)
    00052     {
    00053       stream << result1[i] << ";" << result2[i]<< "\n";
    00054     }
    00055     finished = true;
    00056   }
    00057   locator1->execute();
    00058   pose1 = Pose2D(robotPose);
    00059   locator2->execute();
    00060   pose2 = Pose2D(robotPose);
    00061
    00062   dummy = pose1.translation - currentPose.translation;
    00063   result1[frames] = dummy.abs();
    00064   dummy = pose2.translation - currentPose.translation;
    00065   result2[frames] = dummy.abs();
    00066   frames++;
    00067 }
    00068
    00069 bool ComboLocator::handleMessage(InMessage& message) {
    00070   return false;
    00071 }