HRESULT CDXGraph::Grab(CString strFilePath)
{
if (pControl && pGrabber)
{
long evCode;
double cPosition = 0;
GetCurrentPosition(&cPosition);
//pControl->Stop();
HRESULT hr = pGrabber->SetOneShot(FALSE);
hr = pGrabber->SetBufferSamples(TRUE);
pControl->Run();
//don't add the following code, or it will pause the stream
//pEvent->WaitForCompletion(INFINITE, &evCode);
//Grab it
hr = SetCurrentPosition(cPosition);
//graph must be stopped if you want to call getcurrentbuffer.
pControl->StopWhenReady();
while(TRUE)
{
OAFilterState fs;
pControl->GetState(100, &fs);
if (fs == State_Stopped)
{
break;
}
Sleep(100);
}
long cbBuffer = 0;
hr = pGrabber->GetCurrentBuffer(&cbBuffer, NULL);
char *pBuffer = new char[cbBuffer];
if(!pBuffer)
{
return E_FAIL;
}
hr = pGrabber->GetCurrentBuffer(&cbBuffer, (long*)pBuffer);
//Display it
AM_MEDIA_TYPE mt;
hr = pGrabber->GetConnectedMediaType(&mt);
if (FAILED(hr)) return hr;
VIDEOINFOHEADER *pVih;
if((mt.formattype == FORMAT_VideoInfo) && (mt.cbFormat >= sizeof(VIDEOINFOHEADER)) && (mt.pbFormat != NULL))
{
pVih = (VIDEOINFOHEADER*)mt.pbFormat;
}
else
{
ZeroMemory(&mt, sizeof(AM_MEDIA_TYPE));
return VFW_E_INVALIDMEDIATYPE;
}
//CString fileName;
//fileName.Format(_T("D://Example%d.bmp"), grabCount++);
if(pVih->bmiHeader.biBitCount==32){
HANDLE hf = CreateFile(strFilePath, GENERIC_WRITE, FILE_SHARE_WRITE, NULL, CREATE_ALWAYS, 0, NULL);
if (hf == INVALID_HANDLE_VALUE)
{
return E_FAIL;
}
BITMAPFILEHEADER bfh;
ZeroMemory(&bfh, sizeof(bfh));
bfh.bfType = 'MB';
bfh.bfSize = sizeof(bfh) + cbBuffer*3/4 + sizeof(BITMAPINFOHEADER);
bfh.bfOffBits = sizeof(BITMAPFILEHEADER) +sizeof(BITMAPINFOHEADER);
DWORD dwWritten = 0;
WriteFile(hf, &bfh, sizeof(bfh), &dwWritten, NULL);
BITMAPINFOHEADER bih;
ZeroMemory(&bih, sizeof(bih));
bih.biSize = sizeof(bih);
bih.biWidth = pVih->bmiHeader.biWidth;
bih.biHeight = pVih->bmiHeader.biHeight;
bih.biPlanes = pVih->bmiHeader.biPlanes;
bih.biBitCount = 24 ;//pVih->bmiHeader.biBitCount;
dwWritten = 0;
WriteFile(hf, &bih, sizeof(bih), &dwWritten, NULL);
BYTE* tmpBuf = new BYTE[cbBuffer];
int tmpCbBuffer = 0;
for (int i = 0; i<cbBuffer;i++)
{
if((i&0x3)==0x3){
continue;
}
tmpBuf[tmpCbBuffer++] = pBuffer[i];
}
dwWritten = 0;
WriteFile(hf, tmpBuf, tmpCbBuffer, &dwWritten, NULL);
delete tmpBuf;
CloseHandle(hf);
}
ZeroMemory(&mt, sizeof(AM_MEDIA_TYPE));
delete(pBuffer);
hr = SetCurrentPosition(cPosition);
return hr;
}
return E_FAIL;
}
int32_t WrapperDLL::StreamFile_GetCurrentPosition(void* self){
auto self_ = (StreamFile*)self;
auto ret = self_->GetCurrentPosition();
return ret;
};
bool Actor::IsVisibleFromPoint(Vector3 begin) {
return mGame->GetEngine()->GetPhysics()->CastRay(begin, GetCurrentPosition(), nullptr) == mBody;
}
STDMETHODIMP CStreamSwitcherPassThru::GetCurrentPosition(LONGLONG* pCurrent)
{
CallPeerSeeking(GetCurrentPosition(pCurrent));
}
/*
================
hhCameraInterpolator::GetEyePosition
================
*/
idVec3 hhCameraInterpolator::GetEyePosition() const {
return GetCurrentPosition() + GetCurrentEyeOffset();
}
int main(int argc, char *argv[])
{
int trackerSel;
change_current_dir();
minIni* ini = new minIni(INI_FILE_PATH);
minIni* ini1 = new minIni(M_INI);
StatusCheck::m_ini = ini;
StatusCheck::m_ini1 = ini1;
//////////////////// Framework Initialize ////////////////////////////
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
linux_cm730.SetPortName(U2D_DEV_NAME1);
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
printf("Fail to initialize Motion Manager!\n");
return 0;
}
}
Walking::GetInstance()->LoadINISettings(ini);
usleep(100);
MotionManager::GetInstance()->LoadINISettings(ini);
MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance());
MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance());
//MotionManager::GetInstance()->StartThread();
//LinuxMotionTimer::Initialize(MotionManager::GetInstance());
LinuxMotionTimer linuxMotionTimer;
linuxMotionTimer.Initialize(MotionManager::GetInstance());
linuxMotionTimer.Start();
/////////////////////////////////////////////////////////////////////
// MotionManager::GetInstance()->LoadINISettings(ini);
int firm_ver = 0,retry=0;
//important but allow a few retries
while(cm730.ReadByte(JointData::ID_HEAD_PAN, MX28::P_VERSION, &firm_ver, 0) != CM730::SUCCESS)
{
fprintf(stderr, "Can't read firmware version from Dynamixel ID %d!! \n\n", JointData::ID_HEAD_PAN);
retry++;
if(retry >=3) exit(1);// if we can't do it after 3 attempts its not going to work.
}
if(0 < firm_ver && firm_ver < 40)
{
Action::GetInstance()->LoadFile(MOTION_FILE_PATH);
}
else
{
fprintf(stderr, "Wrong firmware version %d!! \n\n", JointData::ID_HEAD_PAN);
exit(0);
}
//conversion! ////////////////
/*
Action::GetInstance()->LoadFile("../../../Data/motion.bin");
int j,k,p,a;
double f;
for(k=0;k<Action::MAXNUM_PAGE;k++)
{
Action::GetInstance()->LoadPage(k, &Page);
for(j=0;j<Action::MAXNUM_STEP;j++)
{
for(p=0;p<31;p++)
{
a = Page.step[j].position[p];
if(a < 1024)
{
f = ((a-512)*10)/3+2048;
a = (int)f;
if(a<0) a =0;
if(a>4095) a = 4095;
Page.step[j].position[p] = a;
}
}
}
Action::GetInstance()->SavePage(k, &Page);
}
exit(0);
*/
//copy page ////////////////
if(argc>1 && strcmp(argv[1],"-copy")==0)
{
printf("Page copy -- uses files motion_src.bin and motion_dest.bin\n");
if(Action::GetInstance()->LoadFile((char *)"../../../Data/motion_src.bin") == false)
{
printf("Unable to open source file\n");
exit(1);
}
int k;
void *page1;
page1 = malloc(sizeof(Robot::Action::PAGE));
printf("Page to load:");
if(scanf("%d",&k) != EOF)
{
if(Action::GetInstance()->LoadPage(k, (Robot::Action::PAGE *)page1) == false)
{
printf("Unable to load page %d\n",k);
exit(1);
}
if(Action::GetInstance()->LoadFile((char *)"../../../Data/motion_dest.bin") == false)
{
printf("Unable to open destination file\n");
exit(1);
}
if(Action::GetInstance()->SavePage(k, (Robot::Action::PAGE *)page1) == false)
{
printf("Unable to save page %d\n",k);
exit(1);
}
printf("Completed successfully.\n");
exit(0);
}
}
/////////////////////////////
/*
Walking::GetInstance()->m_Joint.SetEnableBody(true,true);
MotionManager::GetInstance()->SetEnable(true);
Walking::GetInstance()->LoadINISettings(m_ini);
cm730.WriteByte(CM730::P_LED_PANNEL, 0x01|0x02|0x04, NULL);
PS3Controller_Start();
LinuxActionScript::PlayMP3("../../../Data/mp3/ready.mp3");
Action::GetInstance()->Start(15);
while(Action::GetInstance()->IsRunning()) usleep(8*1000);
*/
Walking::GetInstance()->LoadINISettings(ini);
MotionManager::GetInstance()->LoadINISettings(ini);
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Head::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBody(true);
MotionManager::GetInstance()->SetEnable(true);
cm730.WriteByte(CM730::P_LED_PANNEL, 0x02, NULL);
if(PS3Controller_Start() == 0)
printf("PS3 controller not installed.\n");
cm730.WriteWord(CM730::P_LED_HEAD_L, cm730.MakeColor(1,1,1),0);
//determine current position
StatusCheck::m_cur_mode = GetCurrentPosition(cm730);
//LinuxActionScript::PlayMP3("../../../Data/mp3/ready.mp3");
if((argc>1 && strcmp(argv[1],"-off")==0) || (StatusCheck::m_cur_mode == SITTING))
{
cm730.DXLPowerOn(false);
//for(int id=JointData::ID_R_SHOULDER_PITCH; id<JointData::NUMBER_OF_JOINTS; id++)
// cm730.WriteByte(id, MX28::P_TORQUE_ENABLE, 0, 0);
}
else
{
Action::GetInstance()->Start(15);
while(Action::GetInstance()->IsRunning()) usleep(8*1000);
}
while(1)
{
StatusCheck::Check(cm730);
if(StatusCheck::m_is_started == 0)
continue;
}
return 0;
}
