Processes/CMD/Debug.cpp

Go to the documentation of this file.

/** 
* @file Processes/CMD/Debug.cpp
* 
* Implementation of class Debug.
*
* @author Martin Lötzsch
*/

#include "Debug.h"
#include "Tools/RobotConfiguration.h"
#include "Tools/Debugging/Debugging.h"
#include "Platform/GTAssert.h"
#include "Platform/SystemCall.h"

Debug::Debug() : 
INIT_EXTERNAL_DEBUGGING,

INIT_RECEIVER_SENSORDATA(false),

INIT_DEBUG_RECEIVER(Cognition),
INIT_DEBUG_RECEIVER(Motion),

INIT_DEBUG_SENDER(Cognition),
INIT_DEBUG_SENDER(Motion),

mode(QueueFillRequest::immediateReadWrite),
saveOrSendTime(0)
{
  //cout << "FREE Memory " << SystemCall::getFreeMem() << "\n" ;
  //cout << flush;
  
  
  debugIn.setSize(400000);
  if (SystemCall::getRobotDesign() == RobotDesign::ERS7)
    debugOut.setSize(9000000);
  else
    debugOut.setSize(1000000);
  
  theCognitionReceiver.setSize(200000);
  theCognitionSender.setSize(400000);
  
//****for GT2005WalkingEngine****
  theMotionReceiver.setSize(40000); // original value 2000
  theMotionSender.setSize(200000);
}

Debug::~Debug() 
{
}

int Debug::main() 
{
  INFO( printRobotStatus, idText, text,
    "Free mem: " << SystemCall::getFreeMem() << " bytes" << endl <<
    "Remaining capacity: " << SystemCall::getRemainingPower() << "%" << endl <<
    "MAC Address: " << getRobotConfiguration().getMacAddressString()
    );
  DEBUG_RESPONSE( "Processes: Debug - printRobotStatus", 
    OUTPUT(idText, text,
    "Free mem: " << SystemCall::getFreeMem() << " bytes" << endl <<
    "Remaining capacity: " << SystemCall::getRemainingPower() << "%" << endl <<
    "MAC Address: " << getRobotConfiguration().getMacAddressString()
    )
    );

  DEBUG_RESPONSE("no wlan:head button saves collected",
    if(theSensorDataBufferReceiver.frame[0].data[SensorData::head] > 10)
    {mode = QueueFillRequest::saveCollected;} );

  DEBUG_RESPONSE("no wlan:back rear saves collected",
    if(theSensorDataBufferReceiver.frame[0].data[SensorData::backR] > 10)
    {mode = QueueFillRequest::saveCollected;} );

  DEBUG_RESPONSE("no wlan:back front starts collecting messages",
    if(theSensorDataBufferReceiver.frame[0].data[SensorData::backF] > 10)
    {mode = QueueFillRequest::collect;} );

  DEBUG_RESPONSE("no wlan:back middle stops collecting messages",
    if(theSensorDataBufferReceiver.frame[0].data[SensorData::backM] > 10)
    {mode = QueueFillRequest::blockNewMessages;} );


  if(
    mode != QueueFillRequest::saveCollected && 
    mode != QueueFillRequest::sendCollected && 
    mode != QueueFillRequest::blockNewMessages ) 
  {
    // add the messages from the queues from the processes to the outgoing queue
    theCognitionReceiver.moveAllMessages(debugOut);
    theMotionReceiver.moveAllMessages(debugOut);
  }

  switch (mode)
  {
  case QueueFillRequest::overwriteOlder:
    debugOut.removeRepetitions();
    break;
  case QueueFillRequest::rejectAll:
    debugOut.clear();
    break;
  case QueueFillRequest::toStickNSeconds:
    if ((SystemCall::getCurrentSystemTime() > saveOrSendTime)&&(saveOrSendTime != 0))
    {
      // save the outgoing queue to the memory stick
      if (!debugOut.isEmpty())
      {
        OutBinaryFile fout("/MS/Logfile.log");
        fout << debugOut;
        debugOut.clear();
      }
      saveOrSendTime = 0;
    }
    break;
  case QueueFillRequest::toStickImmediately:
    // append the outgoing queue to the memory stick
    if (!debugOut.isEmpty())
    {
      OutBinaryFile fout("/MS/Logfile.log",true);
      fout << debugOut;
      debugOut.clear();
    }
    break;
  case QueueFillRequest::saveCollected:
    // append the outgoing queue to the memory stick
    if (!debugOut.isEmpty())
    {
      OutBinaryFile fout("/MS/Logfile.log");
      fout << debugOut;
      debugOut.clear();
    }
    break;
  }
  
  // send the queues to the processes
  theCognitionSender.send();
  theMotionSender.send();
  
#if defined(_WIN32) || defined(WLAN)
  // send the outgoing queue to RobotControl 
  // (direct in WIN32, via TcpGateway in WLAN)
  if ((mode==QueueFillRequest::immediateReadWrite)||
    ((mode==QueueFillRequest::collectNSeconds)&&(saveOrSendTime < SystemCall::getCurrentSystemTime()))||
    (mode==QueueFillRequest::overwriteOlder) ||
    (mode==QueueFillRequest::sendCollected) 
    )
  {
#ifndef _WIN32
    if (messageWasReceived) // Hallo Hack
#endif
      theDebugSender.send();
  }
#endif
  
  return 1; // Wait at least 1 millisecond, then recall main()
}

void Debug::init()
{
  INIT_EXTERNAL_DEBUGGING_HANDLER;
  
  // read requests.dat
  //
  InBinaryFile fin("requests.dat");
  if ((fin.exists())&&(! fin.eof()))
    fin >> debugIn;
  
  debugIn.handleAllMessages(*this);
  debugIn.clear();
  messageWasReceived = false;
}

bool Debug::handleMessage(InMessage& message)
{
  messageWasReceived = true;
  
  switch(message.getMessageID())
  {
  case idText: // loop back to GUI
    message >> debugOut;
    return true;
  case idSensorData:
  case idImage:
  case idJPEGImage:
  case idLowResImage:
  case idPercepts: 
  case idHeadControlMode:   
  case idMotionRequest: 
  case idLEDRequest: 
  case idWorldState: 
  case idColorTable64:
  case idColorTableTSL:
  case idCameraParameters:
  case idOracledWorldState:
  case idRemoteCamWorldState:
  case idGTCamWorldState:
  case idGlobalGameControlData:
  case idGameControlData:
  case idSoundRequest:
  case idSpecialPercept:
  case idImageProcessorParameters:
  case idLinesSelfLocatorParameters:
  case idDDDSelfLocatorParameters:
  case idJoystickData:
  case idGT2004BallLocatorData:
  case idKickSelectionTable:
  case idMotionRatingBehaviorMessage:
    message >> theCognitionSender; 
    return true;
  case idHeadMotionRequest: 
  case idOdometryData: 
  case idJointDataSequence:
  case idFourierCoefficients:
  case idInvKinWalkingParameters:
  case idPIDData:
  case idGameSpeed:
  case idMotionControlSlowMotion:
  case idMotionNet:
  case idOdometryScale:
    message >> theMotionSender;
    return true;
  case idDebugKeyTable: 
    Process::handleMessage(message);
  case idSolutionRequest:
  case idGenericDebugData:
  case idGT2004Parameters:
  case idGT2004EvolutionRequest:
  case idBehaviorEvolutionPopulation:
  case idBehaviorEvolutionParameters:
  case idYetAnotherInvKinParams:
  case idYetAnotherInvKinPaws:
  case idBodyOffsets:
  case idXabsl2DebugRequest:
  case idXabsl2IntermediateCode:
  case idGTCamKalmanBallFilter:
  case idGT2005BallLocator1:
  case idGT2005BallLocator2:
  case idGT2005TeamBallLocator:
  case idMSHBallLocatorEvolution:
  case idComboBallLocator:
  case idDebugDataChangeRequest:
    message >> theCognitionSender; 
    message >> theMotionSender; 
    return true;
  case idDebugRequest:
  {
    message >> theCognitionSender; 
    message >> theMotionSender; 
    return Process::handleMessage(message);
  }

  //****for GT2005WalkingEngine****
  case idGT2005Request:
    message >> theMotionSender; 
    return true;

  case idQueueFillRequest:
    {
      QueueFillRequest qfr;
      message.bin >> qfr;
      mode = qfr.mode;
      saveOrSendTime = 1000*qfr.seconds + SystemCall::getCurrentSystemTime();
      return true;
    }
  default:
    return Process::handleMessage(message);
  }
}

MAKE_PROCESS(Debug);

---