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