Representations/Perception/SpecialPercept.cpp

Go to the documentation of this file.

/**
 * @file SpecialPercept.cpp
 *
 * Implementation of class SpecialPercept.
 *
 * @author <A href=mailto:juengel@informatik.hu-berlin.de>Matthias Juengel</A>, <A href=mailto:brunn@sim.tu-darmstadt.de>Ronnie Brunn</A>
 */

#include "SpecialPercept.h"
#include "SlamPercept.h"
//#include "Tools/FieldDimensions.h"
#include "Platform/SystemCall.h"

BitePoint::BitePoint()
{
  position         = none;
  angleTo          = 0;
  distanceTo       = 0;
  relPos.x         = 0;
  relPos.y         = 0;
  preciseShift     = 0;
  timeWhenLastSeen = 0;
  fresh            = false;
}

void BitePoint::addPercept(double aAngleTo, double aDistanceTo, double aPreciseShift)
{
  angleTo      = aAngleTo;
  distanceTo   = aDistanceTo;
  relPos.x     = sgn(pi_2 - fabs(aAngleTo)) * (cos(fabs(aAngleTo))*aDistanceTo);
  relPos.y     = sgn(aAngleTo) * (sin(fabs(aAngleTo))*aDistanceTo);
  preciseShift = aPreciseShift;

  timeWhenLastSeen = SystemCall::getCurrentSystemTime();

  fresh = true;
}

In& operator>>(In& stream,BitePoint& bitePoint)
{
  stream.read(&(bitePoint.position),         sizeof(BitePoint::Position));
  stream.read(&(bitePoint.angleTo),          sizeof(double));
  stream.read(&(bitePoint.distanceTo),       sizeof(double));
  stream.read(&(bitePoint.relPos),           sizeof(Vector2<double>));
  stream.read(&(bitePoint.preciseShift),     sizeof(double));
  stream.read(&(bitePoint.timeWhenLastSeen), sizeof(unsigned long));
  stream.read(&(bitePoint.fresh),            sizeof(bool));

  return stream;
}
 
Out& operator<<(Out& stream, const BitePoint& bitePoint)
{
  stream.write(&(bitePoint.position),         sizeof(BitePoint::Position));
  stream.write(&(bitePoint.angleTo),          sizeof(double));
  stream.write(&(bitePoint.distanceTo),       sizeof(double));
  stream.write(&(bitePoint.relPos),           sizeof(Vector2<double>));
  stream.write(&(bitePoint.preciseShift),     sizeof(double));
  stream.write(&(bitePoint.timeWhenLastSeen), sizeof(unsigned long));
  stream.write(&(bitePoint.fresh),            sizeof(bool));

  return stream;
}


OCRedLine::OCRedLine(){
  this->seen = false;
  this->angleInImage     =0;
  this->lineStart.x      =0;
  this->lineStart.y      =0;
  this->lineEnd.x        =0;
  this->lineEnd.y        =0;
  this->timeWhenLastSeen =0;

}
void OCRedLine::addPercept(Vector2<int> lineStart, Vector2<int> lineEnd, Vector2<int> pointStart,Vector2<int> pointEnd,int angleInImage){
  this->seen = true;
  this->lineStart=lineStart;
  this->lineEnd=lineEnd;
  this->pointStart=pointStart;
  this->pointEnd=pointEnd;
  this->angleInImage=angleInImage;
  this->timeWhenLastSeen = SystemCall::getCurrentSystemTime();
}


In& operator>>(In& stream,OCRedLine& ocRedLine)
{
  stream.read(&(ocRedLine.lineStart),          sizeof(Vector2<int>));
  stream.read(&(ocRedLine.lineEnd),          sizeof(Vector2<int>));
  stream.read(&(ocRedLine.angleInImage),       sizeof(int));
  stream.read(&(ocRedLine.timeWhenLastSeen), sizeof(unsigned long));

  return stream;
}
 
Out& operator<<(Out& stream, const OCRedLine& ocRedLine)
{
  stream.write(&(ocRedLine.lineStart),          sizeof(Vector2<int>));
  stream.write(&(ocRedLine.lineEnd),          sizeof(Vector2<int>));
  stream.write(&(ocRedLine.angleInImage),       sizeof(int));
  stream.write(&(ocRedLine.timeWhenLastSeen), sizeof(unsigned long));

  return stream;
}


OCBridge::OCBridge()
{
  lastSeenSide     = none;
  angleTo          = 0;
  distanceTo       = 0;
  relPos.x         = 0;
  relPos.y         = 0;
  timeWhenLastSeen = 0;
  fresh            = false;

  for (int i=0; i < BitePoint::numOfPositions; ++i)
    bitePoint[i].position = BitePoint::Position(i);
}

void OCBridge::addPercept(double aAngleTo, double aDistanceTo)
{
  angleTo      = aAngleTo;
  distanceTo   = aDistanceTo;
  relPos.x     = sgn(pi_2 - fabs(aAngleTo)) * (cos(fabs(aAngleTo))*aDistanceTo);
  relPos.y     = sgn(aAngleTo) * (sin(fabs(aAngleTo))*aDistanceTo);

  timeWhenLastSeen = SystemCall::getCurrentSystemTime();

  fresh = true;
}

In& operator>>(In& stream,OCBridge& ocBridge)
{
  stream.read(&(ocBridge.lastSeenSide),     sizeof(OCBridge::Side));
  stream.read(&(ocBridge.angleTo),          sizeof(double));
  stream.read(&(ocBridge.distanceTo),       sizeof(double));
  stream.read(&(ocBridge.relPos),           sizeof(Vector2<double>));
  stream.read(&(ocBridge.timeWhenLastSeen), sizeof(unsigned long));
  stream.read(&(ocBridge.fresh),            sizeof(bool));

  for (int i=0; i < BitePoint::numOfPositions; ++i)
    stream >> ocBridge.bitePoint[i];

  return stream;
}
 
Out& operator<<(Out& stream, const OCBridge& ocBridge)
{
  stream.write(&(ocBridge.lastSeenSide),     sizeof(OCBridge::Side));
  stream.write(&(ocBridge.angleTo),          sizeof(double));
  stream.write(&(ocBridge.distanceTo),       sizeof(double));
  stream.write(&(ocBridge.relPos),           sizeof(Vector2<double>));
  stream.write(&(ocBridge.timeWhenLastSeen), sizeof(unsigned long));
  stream.write(&(ocBridge.fresh),            sizeof(bool));

  for (int i=0; i < BitePoint::numOfPositions; ++i)
    stream << ocBridge.bitePoint[i];

  return stream;
}




SpecialPercept::SpecialPercept()
{
  slamData = new SlamPercept ();

  reset();
}

SpecialPercept::~SpecialPercept()
{
  delete ((SlamPercept*)slamData);
};

void SpecialPercept::reset(unsigned long frameNum)
{
  type = none;
  pan = tilt = 0;
  mostMovement = Vector2<long>(0,0);
  leastMovement = Vector2<long>(0,0);
  checkerPose = Pose2D(0,0,0);
  barCodeId = -1;
  orientation = 0;
  frameNumber = frameNum;
  shutterSelected  = false;
  
  countAngleOfPinkFlags = 0;
  estimatedImageBrightness = 0;
}

In& operator>>(In& stream,SpecialPercept& specialPercept)
{
  specialPercept.reset();
  stream >> specialPercept.frameNumber;
  char temp;
  stream >> temp;
  specialPercept.type = (SpecialPercept::Type)temp;
  switch(specialPercept.type)
  {
  case SpecialPercept::motionDetection: stream >> specialPercept.mostMovement.x >> specialPercept.mostMovement.y >> specialPercept.leastMovement.x >> specialPercept.leastMovement.y;
                                        break;
  case SpecialPercept::checkerboard: stream >> specialPercept.checkerPose.translation.x >> specialPercept.checkerPose.translation.y >> specialPercept.checkerPose.rotation;
                                        break;
  case SpecialPercept::barCode: stream >> specialPercept.barCodeId;
                                        break;
  case SpecialPercept::autoShutter: stream.read(&(specialPercept.shutterSelected), sizeof(bool));
                                        break;
  case SpecialPercept::openChallengeBridge: stream >> specialPercept.ocBridge;
                                        break;
  case SpecialPercept::openChallengeRedLine: stream >> specialPercept.ocRedLine;
                                        break;
  case SpecialPercept::challengeOrientation: stream >> specialPercept.orientation;
                                      break;
  case SpecialPercept::imageBrightness: stream >> specialPercept.estimatedImageBrightness;
                                      break;
  case SpecialPercept::none:            break;
  }

  return stream;
}
 
Out& operator<<(Out& stream, const SpecialPercept& specialPercept)
{
  stream << specialPercept.frameNumber;
  stream << (char)specialPercept.type;
  switch(specialPercept.type)
  {
  case SpecialPercept::motionDetection: stream << specialPercept.mostMovement.x << specialPercept.mostMovement.y << specialPercept.leastMovement.x << specialPercept.leastMovement.y;
                                        break;
  case SpecialPercept::checkerboard: stream << specialPercept.checkerPose.translation.x << specialPercept.checkerPose.translation.y << specialPercept.checkerPose.rotation;
                                        break;
  case SpecialPercept::barCode: stream << specialPercept.barCodeId;
                                        break;
  case SpecialPercept::autoShutter: stream.write(&(specialPercept.shutterSelected), sizeof(bool));
                                        break;
  case SpecialPercept::openChallengeBridge: stream << specialPercept.ocBridge;
                                        break;
  case SpecialPercept::openChallengeRedLine: stream << specialPercept.ocRedLine;
                                        break;
  case SpecialPercept::challengeOrientation: stream << specialPercept.orientation;
  case SpecialPercept::none:            break;
  }
  
  return stream;
}

---