int main(void) { printf( "\n===== Action script Tutorial for DARwIn =====\n\n"); minIni* ini = new minIni(INI_FILE_PATH); change_current_dir(); Action::GetInstance()->LoadFile(MOTION_FILE_PATH); //////////////////// Framework Initialize //////////////////////////// LinuxCM730 linux_cm730("/dev/ttyUSB0"); CM730 cm730(&linux_cm730); if(MotionManager::GetInstance()->Initialize(&cm730) == false) { printf("Fail to initialize Motion Manager!\n"); return 0; } MotionManager::GetInstance()->LoadINISettings(ini); MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance()); LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance()); motion_timer->Start(); ///////////////////////////////////////////////////////////////////// MotionManager::GetInstance()->SetEnable(true); Action::GetInstance()->Start(1); /* Init(stand up) pose */ while(Action::GetInstance()->IsRunning()) usleep(8*1000); printf("Press the ENTER key to begin!\n"); getchar(); LinuxActionScript::ScriptStart("script.asc"); while(LinuxActionScript::m_is_running == 1) sleep(10); return 0; }
int main(void) { printf( "\n===== Head tracking Tutorial for DARwIn =====\n\n"); change_current_dir(); minIni* ini = new minIni(INI_FILE_PATH); Image* rgb_ball = new Image(Camera::WIDTH, Camera::HEIGHT, Image::RGB_PIXEL_SIZE); LinuxCamera::GetInstance()->Initialize(0); LinuxCamera::GetInstance()->LoadINISettings(ini); mjpg_streamer* streamer = new mjpg_streamer(Camera::WIDTH, Camera::HEIGHT); ColorFinder* ball_finder = new ColorFinder(); ball_finder->LoadINISettings(ini); httpd::ball_finder = ball_finder; BallTracker tracker = BallTracker(); //////////////////// Framework Initialize //////////////////////////// LinuxCM730 linux_cm730(U2D_DEV_NAME); CM730 cm730(&linux_cm730); if(MotionManager::GetInstance()->Initialize(&cm730) == false) { printf("Fail to initialize Motion Manager!\n"); return 0; } MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance()); LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance()); motion_timer->Start(); MotionStatus::m_CurrentJoints.SetEnableBodyWithoutHead(false); MotionManager::GetInstance()->SetEnable(true); ///////////////////////////////////////////////////////////////////// Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true); Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_PAN, 8); Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_TILT, 8); struct timeval tval; gettimeofday(&tval, NULL); double prevtime = tval.tv_sec + tval.tv_usec / 1000000.0; while(1) { Point2D pos; LinuxCamera::GetInstance()->CaptureFrame(); tracker.Process(ball_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame)); rgb_ball = LinuxCamera::GetInstance()->fbuffer->m_RGBFrame; for(int i = 0; i < rgb_ball->m_NumberOfPixels; i++) { if(ball_finder->m_result->m_ImageData[i] == 1) { rgb_ball->m_ImageData[i*rgb_ball->m_PixelSize + 0] = 255; rgb_ball->m_ImageData[i*rgb_ball->m_PixelSize + 1] = 0; rgb_ball->m_ImageData[i*rgb_ball->m_PixelSize + 2] = 0; } } streamer->send_image(rgb_ball); gettimeofday(&tval, NULL); double curtime = tval.tv_sec + tval.tv_usec / 1000000.0; printf("FPS: % 2d\n", (int) (1 / (curtime - prevtime))); prevtime = curtime; } return 0; }
int main() { LinuxCM730 linux_cm730("/dev/ttyUSB0"); CM730 cm730(&linux_cm730); actionEditor* ae = new actionEditor(&cm730); /* getchar(); while(!ae->turnOnOff(0)); ae->setPage(100); getchar(); while(!ae->turnOnOff(ALL)); printf("on\n"); ae->printAll(); getchar(); while(!ae->turnOnOff(0)); printf("off\n"); getchar(); printf("input pageNo"); int pageNo; std::cin >> pageNo; ae->setPage(pageNo); getchar(); printf("set 7 step\n"); for(int i = 0; i < 7; i++) { while(!ae->turnOnOff(0)); printf("turn off, press any key to form step\n"); getchar(); while(!ae->turnOnOff(ALL)); printf("turn on, press any key\n"); getchar(); ae->readStep(); ae->printAll(); ae->writeStep(i); } ae->saveFile(); ae->turnOnOff(0); */ getchar(); ae->setPage(100); ae->setStepNum(5); char* name = new char[10]; name[0] = 't'; name[1] = 'e'; name[2] = 's'; name[3] = 't'; name[4] = 0; ae->setPageName(name); for(int i = 0; i < 5; i++) { printf("press to pre form step %d\n", i); getchar(); ae->preFormStep(0); printf("press to form step %d\n", i); getchar(); ae->formStep(i, 100, 100); } ae->saveFile(); printf("press to play\n"); getchar(); ae->play(); //ae->playStep(2); delete ae; return 0; }
int main(void) { printf( "\n===== Action script Tutorial for DARwIn =====\n\n"); change_current_dir(); Action::GetInstance()->LoadFile(MOTION_FILE_PATH); //////////////////// Framework Initialize //////////////////////////// LinuxCM730 linux_cm730("/dev/ttyUSB0"); CM730 cm730(&linux_cm730); if(MotionManager::GetInstance()->Initialize(&cm730) == false) { printf("Fail to initialize Motion Manager!\n"); return 0; } MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance()); LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance()); motion_timer->Start(); ///////////////////////////////////////////////////////////////////// MotionManager::GetInstance()->SetEnable(true); //Action* Action::m_UniqueInstance = new Action(); Action::GetInstance()->m_Joint.SetEnableBody(true, true);// this opposes to the torque disable // Action::GetInstance()->Start(1); usleep(1000); Action::GetInstance()->Start(7); /* Init(stand up) pose */ //Action::GetInstance()->Start(1); while(Action::GetInstance()->IsRunning()) usleep(8*1000); printf("Press the ENTER key to begin!\n"); getchar(); LinuxActionScript::m_is_running = 0; //Action::GetInstance()->Start(8); //Action::GetInstance()->m_Joint.SetAngle(6, MotionStatus::m_CurrentJoints.GetValue(6)); //leftarm = JointData::GetAngle(int ID_R_SHOULDER_PITCH); //Action::GetInstance()->m_Joint.SetAngle(5, MotionStatus::m_CurrentJoints.GetValue(5)); //leftarm = MotionStatus::m_CurrentJoints.GetAngle(6); //rightarm = MotionStatus::m_CurrentJoints.GetAngle(5); //cm730.WriteWord(JointData::ID_R_SHOULDER_PITCH, MX28::P_TORQUE_ENABLE, 0, 0); //cm730.WriteWord(JointData::ID_R_SHOULDER_ROLL, MX28::P_TORQUE_ENABLE, 0, 0); //cm730.WriteWord(JointData::ID_R_ELBOW, MX28::P_TORQUE_ENABLE, 0, 0); //Action::GetInstance()->m_Joint.SetValue(1, 2000); //Action::GetInstance()->m_Joint.SetValue(2, 2000); //while(Action::GetInstance()->IsRunning()) usleep(8000); //Action::GetInstance()->m_Joint.SetAngle(6, -20); //Action::GetInstance()->m_Joint.SetAngle(5, 20); // LinuxActionScript::ScriptStart("script.asc"); while(LinuxActionScript::m_is_running == 1) sleep(10); return 0; }
int main(int argc, const char** argv) { printf( "\n===== Face tracking Tutorial for DARwIn =====\n\n"); change_current_dir(); minIni* ini = new minIni(INI_FILE_PATH); Image* rgb_ball = new Image(Camera::WIDTH, Camera::HEIGHT, Image::RGB_PIXEL_SIZE); LinuxCamera::GetInstance()->Initialize(0); LinuxCamera::GetInstance()->LoadINISettings(ini); mjpg_streamer* streamer = new mjpg_streamer(Camera::WIDTH, Camera::HEIGHT); BallTracker tracker = BallTracker(); //////////////////// Framework Initialize //////////////////////////// LinuxCM730 linux_cm730(U2D_DEV_NAME); CM730 cm730(&linux_cm730); if(MotionManager::GetInstance()->Initialize(&cm730) == false) { printf("Fail to initialize Motion Manager!\n"); // return 0; } MotionManager::GetInstance()->LoadINISettings(ini); MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance()); LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance()); motion_timer->Start(); MotionStatus::m_CurrentJoints.SetEnableBodyWithoutHead(false); MotionManager::GetInstance()->SetEnable(true); ///////////////////////////////////////////////////////////////////// //Head::GetInstance()->LoadINISettings(ini); Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true); Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_PAN, 8); Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_TILT, 8); printf("Press the ENTER key to begin!\n"); getchar(); //parse command line arguments char ftFile[256],conFile[256],triFile[256]; bool fcheck = false; double scale = 1; int fpd = -1; bool show = true; if(parse_cmd(argc,argv,ftFile,conFile,triFile,fcheck,scale,fpd)<0)return 0; //set other tracking parameters std::vector<int> wSize1(1); wSize1[0] = 7; std::vector<int> wSize2(3); wSize2[0] = 11; wSize2[1] = 9; wSize2[2] = 7; int nIter = 5; double clamp=3,fTol=0.01; FACETRACKER::Tracker model(ftFile); cv::Mat tri=FACETRACKER::IO::LoadTri(triFile); cv::Mat con=FACETRACKER::IO::LoadCon(conFile); //initialize camera and display window cv::Mat frame,gray,im; double fps=0; char sss[256]; std::string text; int64 t1,t0 = cvGetTickCount(); int fnum=0; bool failed = true; while(1) { Point2D pos; LinuxCamera::GetInstance()->CaptureFrame(); //Create the image header which will contain the converted BYTE image int channel = 3; //1 for black and white pictures, 3 for colored pictures int step = LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_WidthStep;//depends from the channel //grab image, resize and flip IplImage* I = cvCreateImageHeader(cv::Size(LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_Width,LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_Height), IPL_DEPTH_8U, channel); //Set the BYTE pointer as attribut of the IplImage --> "converts" BYTE *pData in IplImage *frame cvSetData(I, LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_ImageData, step); if(!I)continue; frame = I; if(scale == 1)im = frame; else cv::resize(frame,im,cv::Size(scale*frame.cols,scale*frame.rows)); cv::flip(im,im,1); cv::cvtColor(im,gray,CV_RGB2GRAY); //track this image std::vector<int> wSize; if(failed)wSize = wSize2; else wSize = wSize1; if(model.Track(gray,wSize,fpd,nIter,clamp,fTol,fcheck) == 0){ int idx = model._clm.GetViewIdx(); failed = false; Draw(im,model._shape,con,tri,model._clm._visi[idx]); pos.X = model._rect.x+ model._rect.width/2; pos.Y = model._rect.y+ model._rect.height/2; }else{ if(show){cv::Mat R(im,cvRect(0,0,150,50)); R = cv::Scalar(0,0,255);} model.FrameReset(); failed = true; pos.X = -1; pos.Y = -1; } //draw framerate on display image if(fnum >= 9){ t1 = cvGetTickCount(); fps = 10.0/((double(t1-t0)/cvGetTickFrequency())/1e+6); t0 = t1; fnum = 0; }else fnum += 1; if(show){ sprintf(sss,"%d frames/sec",(int)round(fps)); text = sss; cv::putText(im,text,cv::Point(10,20), CV_FONT_HERSHEY_SIMPLEX,0.5,CV_RGB(255,255,255)); } //show image and check for user input imshow("Face Tracker",im); int c = cv::waitKey(10); if(c == 27)break; else if(char(c) == 'd')model.Save("/home/koreki/Documents/perso/darwin/Linux/project/tutorial/face_tracker/koreki.tracker");//model.FrameReset(); //model.FrameReset(); // pos = facefinder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_RGBFrame); tracker.Process(pos); fprintf(stderr, "posx: %f, posy: %f \r", pos.X, pos.Y); rgb_ball = LinuxCamera::GetInstance()->fbuffer->m_RGBFrame; rgb_ball->m_ImageData = im.data; streamer->send_image(rgb_ball); } return 0; }