AiboLib: CamExample.cc

#include "CamExample.h"

#include <OPENR/ODataFormats.h>
#include <OPENR/OPENRAPI.h>
#include <OPENR/OSyslog.h>
#include <OPENR/core_macro.h>


CamExample::CamExample() : State(SYSTEM_IDLE)
{
  CamCounter = 0;
};

OStatus CamExample::DoInit(const OSystemEvent& event)
{
  OSYSDEBUG(("CamExample::DoInit() \n"));
  
  Camera.OpenPrimitive();

  NEW_ALL_SUBJECT_AND_OBSERVER;
  REGISTER_ALL_ENTRY;
  SET_ALL_READY_AND_NOTIFY_ENTRY;
  
  // done in LedsExample
  // MotorPowerOn();
  
  return oSUCCESS;
};

OStatus CamExample::DoStart(const OSystemEvent& event)
{
  OSYSDEBUG(("CamExample::DoStart()\n"));
  
  State = SYSTEM_START;
  
  ENABLE_ALL_SUBJECT;
  ASSERT_READY_TO_ALL_OBSERVER;

  return oSUCCESS;
};

OStatus CamExample::DoStop(const OSystemEvent& event)
{
  
  OSYSDEBUG(("CamExample::DoStop()\n"));

  DISABLE_ALL_SUBJECT;
  DEASSERT_READY_TO_ALL_OBSERVER;
  return oSUCCESS;
};

OStatus CamExample::DoDestroy(const OSystemEvent& event)
{
  OSYSDEBUG(("CamExample::DoDestroy()\n"));

  DELETE_ALL_SUBJECT_AND_OBSERVER;
  return oSUCCESS;
};


void CamExample::NotifyCam(const ONotifyEvent& event)
{
  OSYSDEBUG(("CamExample::NotifyCam()\n"));
  char name[128];

  if(State == SYSTEM_START){

    OFbkImageVectorData* fbkImageVectorData  = (OFbkImageVectorData*)event.Data(0);
    
    if (CamCounter < 5) {
      
      OSYSPRINT(("Saving image %d \n", CamCounter));

      sprintf(name, "/MS/OPEN-R/MW/DATA/P/RGBH%04d.BMP", CamCounter);
      Camera.SaveBMP(name, fbkImageVectorData, ofbkimageLAYER_H);

      sprintf(name, "/MS/OPEN-R/MW/DATA/P/RGBM%04d.BMP", CamCounter);
      Camera.SaveBMP(name, fbkImageVectorData, ofbkimageLAYER_M);
      
      sprintf(name, "/MS/OPEN-R/MW/DATA/P/RGBL%04d.BMP", CamCounter);
      Camera.SaveBMP(name, fbkImageVectorData, ofbkimageLAYER_L);

      sprintf(name, "/MS/OPEN-R/MW/DATA/P/RGBL%04d.raw", CamCounter);
      Camera.SaveRawData(name, fbkImageVectorData, ofbkimageLAYER_H);

      CamCounter++;
    } 
    else {
      OSYSPRINT(("Done! Switch off! ... \n"));
    }
  }
  else{
    OSYSDEBUG(("---NotifyCam:Nix\n"));
  }

  observer[event.ObsIndex()]->AssertReady(event.SenderID());
};