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 "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()
{
  reset();
}

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

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

/*
 * Change log :
 * 
 * $Log: SpecialPercept.cpp,v $
 * Revision 1.5  2004/07/01 18:42:08  hamerla
 * SLAM challenge find Orientation
 *
 * Revision 1.4  2004/06/29 15:10:57  kerdels
 * Special Vision AutoShutter...
 *
 * Revision 1.3  2004/06/28 12:58:17  wachter
 * Added special-percept for challenge and corresponding symbols.
 *
 * Revision 1.2  2004/06/01 13:58:14  schumann
 * special percept type redLine added,
 * part 1 of open challenge added
 *
 * Revision 1.1.1.1  2004/05/22 17:26:03  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.7  2004/05/20 16:30:03  kerdels
 * further work on open challenge
 *
 * Revision 1.6  2004/05/17 01:56:16  kerdels
 * prepared some symbols for the open challenge bridge detection
 *
 * Revision 1.5  2004/01/19 17:25:37  dueffert
 * frameNumber is now streamed in all cases
 *
 * Revision 1.4  2004/01/19 14:53:46  dueffert
 * all frameNumbers (and not only some of them) are unsigned long now
 *
 * Revision 1.3  2003/11/14 19:02:26  goehring
 * frameNumber added
 *
 * Revision 1.2  2003/11/10 13:29:45  dueffert
 * frame number added
 *
 * Revision 1.1  2003/10/07 10:09:36  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.2  2003/07/30 14:55:54  dueffert
 * checkerboard percept improved
 *
 * Revision 1.1.1.1  2003/07/02 09:40:22  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.10  2003/05/26 08:12:10  juengel
 * Moved angleToFreePartOfGoal from specialPercept to obstaclesPercept.
 *
 * Revision 1.9  2003/05/01 10:20:58  roefer
 * Added distance and width for freePartOfO*Goal
 *
 * Revision 1.8  2003/04/23 16:12:03  juengel
 * Added angleToFreePartOfOwnGoal.
 *
 * Revision 1.7  2003/04/05 16:46:24  juengel
 * Added freePartOfOpponentGoal to streaming operator.
 *
 * Revision 1.6  2003/04/02 15:36:01  dueffert
 * new symbols added
 *
 * Revision 1.5  2003/03/06 11:48:48  dueffert
 * todo comments for missing cases added
 *
 * Revision 1.4  2003/01/22 15:20:25  dueffert
 * checkerboard stuff added
 *
 * Revision 1.3  2002/11/28 14:43:48  jhoffman
 * added a special percept for the motion detector
 *
 * Revision 1.2  2002/09/25 12:05:43  loetzsch
 * removed BarPercept and PatternPercept
 *
 * Revision 1.1  2002/09/10 15:26:40  cvsadm
 * Created new project GT2003 (M.L.)
 * - Cleaned up the /Src/DataTypes directory
 * - Removed Challenge Code
 * - Removed processing of incoming audio data
 * - Renamed AcousticMessage to SoundRequest
 *
 * Revision 1.5  2002/08/30 13:33:05  dueffert
 * removed unused includes
 *
 * Revision 1.4  2002/05/17 11:51:48  brunn
 * first experimental version of the barPerceptor
 *
 * Revision 1.3  2002/05/15 11:24:31  brunn
 * barEnd poses instead of complete bar pose
 * and initialization added
 *
 * Revision 1.2  2002/05/11 06:03:32  juengel
 * Constructor for PatternPercept added.
 *
 * Revision 1.1  2002/05/10 17:40:26  juengel
 * Added SpecialVision and SpecialPercept.
 *
 */

---