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

# Representations/Perception/SpecialPercept.cpp

Go to the documentation of this file.

    00001 /**
    00002  * @file SpecialPercept.cpp
    00003  *
    00004  * Implementation of class SpecialPercept.
    00005  *
    00006  * @author <A href=mailto:juengel@informatik.hu-berlin.de>Matthias Juengel</A>, <A href=mailto:brunn@sim.tu-darmstadt.de>Ronnie Brunn</A>
    00007  */
    00008
    00009 #include "SpecialPercept.h"
    00010 #include "SlamPercept.h"
    00011 //#include "Tools/FieldDimensions.h"
    00012 #include "Platform/SystemCall.h"
    00013
    00014 BitePoint::BitePoint()
    00015 {
    00016   position         = none;
    00017   angleTo          = 0;
    00018   distanceTo       = 0;
    00019   relPos.x         = 0;
    00020   relPos.y         = 0;
    00021   preciseShift     = 0;
    00022   timeWhenLastSeen = 0;
    00023   fresh            = false;
    00024 }
    00025
    00026 void BitePoint::addPercept(double aAngleTo, double aDistanceTo, double aPreciseShift)
    00027 {
    00028   angleTo      = aAngleTo;
    00029   distanceTo   = aDistanceTo;
    00030   relPos.x     = sgn(pi_2 - fabs(aAngleTo)) * (cos(fabs(aAngleTo))*aDistanceTo);
    00031   relPos.y     = sgn(aAngleTo) * (sin(fabs(aAngleTo))*aDistanceTo);
    00032   preciseShift = aPreciseShift;
    00033
    00034   timeWhenLastSeen = SystemCall::getCurrentSystemTime();
    00035
    00036   fresh = true;
    00037 }
    00038
    00039 In& operator>>(In& stream,BitePoint& bitePoint)
    00040 {
    00041   stream.read(&(bitePoint.position),         sizeof(BitePoint::Position));
    00042   stream.read(&(bitePoint.angleTo),          sizeof(double));
    00043   stream.read(&(bitePoint.distanceTo),       sizeof(double));
    00044   stream.read(&(bitePoint.relPos),           sizeof(Vector2<double>));
    00045   stream.read(&(bitePoint.preciseShift),     sizeof(double));
    00046   stream.read(&(bitePoint.timeWhenLastSeen), sizeof(unsigned long));
    00047   stream.read(&(bitePoint.fresh),            sizeof(bool));
    00048
    00049   return stream;
    00050 }
    00051
    00052 Out& operator<<(Out& stream, const BitePoint& bitePoint)
    00053 {
    00054   stream.write(&(bitePoint.position),         sizeof(BitePoint::Position));
    00055   stream.write(&(bitePoint.angleTo),          sizeof(double));
    00056   stream.write(&(bitePoint.distanceTo),       sizeof(double));
    00057   stream.write(&(bitePoint.relPos),           sizeof(Vector2<double>));
    00058   stream.write(&(bitePoint.preciseShift),     sizeof(double));
    00059   stream.write(&(bitePoint.timeWhenLastSeen), sizeof(unsigned long));
    00060   stream.write(&(bitePoint.fresh),            sizeof(bool));
    00061
    00062   return stream;
    00063 }
    00064
    00065
    00066 OCRedLine::OCRedLine(){
    00067   this->seen = false;
    00068   this->angleInImage     =0;
    00069   this->lineStart.x      =0;
    00070   this->lineStart.y      =0;
    00071   this->lineEnd.x        =0;
    00072   this->lineEnd.y        =0;
    00073   this->timeWhenLastSeen =0;
    00074
    00075 }
    00076 void OCRedLine::addPercept(Vector2<int> lineStart, Vector2<int> lineEnd, Vector2<int> pointStart,Vector2<int> pointEnd,int angleInImage){
    00077   this->seen = true;
    00078   this->lineStart=lineStart;
    00079   this->lineEnd=lineEnd;
    00080   this->pointStart=pointStart;
    00081   this->pointEnd=pointEnd;
    00082   this->angleInImage=angleInImage;
    00083   this->timeWhenLastSeen = SystemCall::getCurrentSystemTime();
    00084 }
    00085
    00086
    00087 In& operator>>(In& stream,OCRedLine& ocRedLine)
    00088 {
    00089   stream.read(&(ocRedLine.lineStart),          sizeof(Vector2<int>));
    00090   stream.read(&(ocRedLine.lineEnd),          sizeof(Vector2<int>));
    00091   stream.read(&(ocRedLine.angleInImage),       sizeof(int));
    00092   stream.read(&(ocRedLine.timeWhenLastSeen), sizeof(unsigned long));
    00093
    00094   return stream;
    00095 }
    00096
    00097 Out& operator<<(Out& stream, const OCRedLine& ocRedLine)
    00098 {
    00099   stream.write(&(ocRedLine.lineStart),          sizeof(Vector2<int>));
    00100   stream.write(&(ocRedLine.lineEnd),          sizeof(Vector2<int>));
    00101   stream.write(&(ocRedLine.angleInImage),       sizeof(int));
    00102   stream.write(&(ocRedLine.timeWhenLastSeen), sizeof(unsigned long));
    00103
    00104   return stream;
    00105 }
    00106
    00107
    00108 OCBridge::OCBridge()
    00109 {
    00110   lastSeenSide     = none;
    00111   angleTo          = 0;
    00112   distanceTo       = 0;
    00113   relPos.x         = 0;
    00114   relPos.y         = 0;
    00115   timeWhenLastSeen = 0;
    00116   fresh            = false;
    00117
    00118   for (int i=0; i < BitePoint::numOfPositions; ++i)
    00119     bitePoint[i].position = BitePoint::Position(i);
    00120 }
    00121
    00122 void OCBridge::addPercept(double aAngleTo, double aDistanceTo)
    00123 {
    00124   angleTo      = aAngleTo;
    00125   distanceTo   = aDistanceTo;
    00126   relPos.x     = sgn(pi_2 - fabs(aAngleTo)) * (cos(fabs(aAngleTo))*aDistanceTo);
    00127   relPos.y     = sgn(aAngleTo) * (sin(fabs(aAngleTo))*aDistanceTo);
    00128
    00129   timeWhenLastSeen = SystemCall::getCurrentSystemTime();
    00130
    00131   fresh = true;
    00132 }
    00133
    00134 In& operator>>(In& stream,OCBridge& ocBridge)
    00135 {
    00136   stream.read(&(ocBridge.lastSeenSide),     sizeof(OCBridge::Side));
    00137   stream.read(&(ocBridge.angleTo),          sizeof(double));
    00138   stream.read(&(ocBridge.distanceTo),       sizeof(double));
    00139   stream.read(&(ocBridge.relPos),           sizeof(Vector2<double>));
    00140   stream.read(&(ocBridge.timeWhenLastSeen), sizeof(unsigned long));
    00141   stream.read(&(ocBridge.fresh),            sizeof(bool));
    00142
    00143   for (int i=0; i < BitePoint::numOfPositions; ++i)
    00144     stream >> ocBridge.bitePoint[i];
    00145
    00146   return stream;
    00147 }
    00148
    00149 Out& operator<<(Out& stream, const OCBridge& ocBridge)
    00150 {
    00151   stream.write(&(ocBridge.lastSeenSide),     sizeof(OCBridge::Side));
    00152   stream.write(&(ocBridge.angleTo),          sizeof(double));
    00153   stream.write(&(ocBridge.distanceTo),       sizeof(double));
    00154   stream.write(&(ocBridge.relPos),           sizeof(Vector2<double>));
    00155   stream.write(&(ocBridge.timeWhenLastSeen), sizeof(unsigned long));
    00156   stream.write(&(ocBridge.fresh),            sizeof(bool));
    00157
    00158   for (int i=0; i < BitePoint::numOfPositions; ++i)
    00159     stream << ocBridge.bitePoint[i];
    00160
    00161   return stream;
    00162 }
    00163
    00164
    00165
    00166
    00167 SpecialPercept::SpecialPercept()
    00168 {
    00169   slamData = new SlamPercept ();
    00170
    00171   reset();
    00172 }
    00173
    00174 SpecialPercept::~SpecialPercept()
    00175 {
    00176   delete ((SlamPercept*)slamData);
    00177 };
    00178
    00179 void SpecialPercept::reset(unsigned long frameNum)
    00180 {
    00181   type = none;
    00182   pan = tilt = 0;
    00183   mostMovement = Vector2<long>(0,0);
    00184   leastMovement = Vector2<long>(0,0);
    00185   checkerPose = Pose2D(0,0,0);
    00186   barCodeId = -1;
    00187   orientation = 0;
    00188   frameNumber = frameNum;
    00189   shutterSelected  = false;
    00190
    00191   countAngleOfPinkFlags = 0;
    00192   estimatedImageBrightness = 0;
    00193 }
    00194
    00195 In& operator>>(In& stream,SpecialPercept& specialPercept)
    00196 {
    00197   specialPercept.reset();
    00198   stream >> specialPercept.frameNumber;
    00199   char temp;
    00200   stream >> temp;
    00201   specialPercept.type = (SpecialPercept::Type)temp;
    00202   switch(specialPercept.type)
    00203   {
    00204   case SpecialPercept::motionDetection: stream >> specialPercept.mostMovement.x >> specialPercept.mostMovement.y >> specialPercept.leastMovement.x >> specialPercept.leastMovement.y;
    00205                                         break;
    00206   case SpecialPercept::checkerboard: stream >> specialPercept.checkerPose.translation.x >> specialPercept.checkerPose.translation.y >> specialPercept.checkerPose.rotation;
    00207                                         break;
    00208   case SpecialPercept::barCode: stream >> specialPercept.barCodeId;
    00209                                         break;
    00210   case SpecialPercept::autoShutter: stream.read(&(specialPercept.shutterSelected), sizeof(bool));
    00211                                         break;
    00212   case SpecialPercept::openChallengeBridge: stream >> specialPercept.ocBridge;
    00213                                         break;
    00214   case SpecialPercept::openChallengeRedLine: stream >> specialPercept.ocRedLine;
    00215                                         break;
    00216   case SpecialPercept::challengeOrientation: stream >> specialPercept.orientation;
    00217                                       break;
    00218   case SpecialPercept::imageBrightness: stream >> specialPercept.estimatedImageBrightness;
    00219                                       break;
    00220   case SpecialPercept::none:            break;
    00221   }
    00222
    00223   return stream;
    00224 }
    00225
    00226 Out& operator<<(Out& stream, const SpecialPercept& specialPercept)
    00227 {
    00228   stream << specialPercept.frameNumber;
    00229   stream << (char)specialPercept.type;
    00230   switch(specialPercept.type)
    00231   {
    00232   case SpecialPercept::motionDetection: stream << specialPercept.mostMovement.x << specialPercept.mostMovement.y << specialPercept.leastMovement.x << specialPercept.leastMovement.y;
    00233                                         break;
    00234   case SpecialPercept::checkerboard: stream << specialPercept.checkerPose.translation.x << specialPercept.checkerPose.translation.y << specialPercept.checkerPose.rotation;
    00235                                         break;
    00236   case SpecialPercept::barCode: stream << specialPercept.barCodeId;
    00237                                         break;
    00238   case SpecialPercept::autoShutter: stream.write(&(specialPercept.shutterSelected), sizeof(bool));
    00239                                         break;
    00240   case SpecialPercept::openChallengeBridge: stream << specialPercept.ocBridge;
    00241                                         break;
    00242   case SpecialPercept::openChallengeRedLine: stream << specialPercept.ocRedLine;
    00243                                         break;
    00244   case SpecialPercept::challengeOrientation: stream << specialPercept.orientation;
    00245   case SpecialPercept::none:            break;
    00246   }
    00247
    00248   return stream;
    00249 }