Exemple #1
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);

    BallTracker tracker = BallTracker();
    tracker.LoadINISettings(ini);
    httpd::ball_finder = &tracker.finder;

    //////////////////// Framework Initialize ////////////////////////////
    LinuxArbotixPro linux_arbotixpro(U2D_DEV_NAME);
    ArbotixPro arbotixpro(&linux_arbotixpro);
    if (MotionManager::GetInstance()->Initialize(&arbotixpro) == false)
        {
            printf("Fail to initialize Motion Manager!\n");
            return 0;
        }
    MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
    LinuxMotionTimer::Initialize(MotionManager::GetInstance());

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

    while (1)
        {
            Point2D pos;
            LinuxCamera::GetInstance()->CaptureFrame();

            tracker.Process(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);

            rgb_ball = LinuxCamera::GetInstance()->fbuffer->m_RGBFrame;
            for (int i = 0; i < rgb_ball->m_NumberOfPixels; i++)
                {
                    if (tracker.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);
        }

    return 0;
}
Exemple #2
0
int main(void)
{
    printf( "\n===== Camera Tutorial for DARwIn =====\n\n");

    change_current_dir();

    minIni* ini = new minIni(INI_FILE_PATH);

    LinuxCamera::GetInstance()->Initialize(0);
    LinuxCamera::GetInstance()->LoadINISettings(ini);

    mjpg_streamer* streamer = new mjpg_streamer(Camera::WIDTH, Camera::HEIGHT);

    while (1)
        {
            LinuxCamera::GetInstance()->CaptureFrame();
            streamer->send_image(LinuxCamera::GetInstance()->fbuffer->m_YUVFrame);
        }

    return 0;
}
Exemple #3
0
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;
}
Exemple #4
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(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;
}
Exemple #6
0
int main(void)
{
    time_t rawtime;
    struct tm * timeinfo;
    time ( &rawtime );
    timeinfo = localtime ( &rawtime );
    stringstream p;
    p<<"[" << timeinfo->tm_hour<< ":"<< timeinfo->tm_min << ":" << timeinfo->tm_sec <<"]";
	std::string logname = "/home/robot/Desktop/LOG";
	logname+=p.str();
    startLog(logFile,logname);

	for (int i=0; i<BrainBall.x_detail ;i++)
	{
		BrainBall.x_place[i] = 0;
	}



    printf( "\n===== Action script for DARwIn =====\n\n");
    change_current_dir();
    minIni* ini = new minIni(INI_FILE_PATH);
    Action::GetInstance()->LoadFile(MOTION_FILE_PATH);

    //////////////////// Framework Initialize ////////////////////////////
    if(MotionManager::GetInstance()->Initialize(&cm730) == false)
    {
		linux_cm730.SetPortName("/dev/ttyUSB1");
		if(MotionManager::GetInstance()->Initialize(&cm730) == false)
			{
				printf("Fail to initialize Motion Manager!\n");
				return 0;
			}
    }
    Walking::GetInstance()->LoadINISettings(ini);
    MotionManager::GetInstance()->LoadINISettings(ini);
	
    MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance());
    MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
    MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance());	
    LinuxMotionTimer linuxMotionTimer;
    linuxMotionTimer.Initialize(MotionManager::GetInstance());
    linuxMotionTimer.Start();
    /////////////////////////////////////////////////////////////////////

    Walking::GetInstance()->m_Joint.SetEnableBody(false);
    Head::GetInstance()->m_Joint.SetEnableBody(false);
    Action::GetInstance()->m_Joint.SetEnableBody(true);
    MotionManager::GetInstance()->SetEnable(true);

    Action::GetInstance()->Start(1);     //Init(stand up) pose

    Action::GetInstance()->Brake();
    while(Action::GetInstance()->IsRunning()) usleep(8*1000);

    cm730.WriteByte(CM730::P_LED_PANNEL, 0x02, NULL);
   MotionManager::GetInstance()->ResetGyroCalibration();

   printf("Press the ENTER key to begin!\n");
   getchar();

//////
    writeToLog(logFile,"started");

printf("0-Play\n");
printf("1-Kick\n");
printf("2-stand\n");
printf("3-Straight walking\n");
printf("4-Turn right\n");
printf("5-Turn left\n");
printf("6-stop walking\n");
printf("7-reset\n");
printf("8-GetTilt\n");
printf("9-SetTilt\n");
printf("q-Exit\n");

Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);

	while(1)
	{
		char c =getchar();

		if (c == '0')
		{
			play();
		}
		else if (c == '1')
		{
			writeToLog(logFile,"kick");
			Action::GetInstance()->Start(2);
			while(Action::GetInstance()->IsRunning()) usleep(8*1000);
		}
		else if(c == '2')
		{
			writeToLog(logFile,"stand");
			Action::GetInstance()->Start(1);
			while(Action::GetInstance()->IsRunning()) usleep(8*1000);
		}
		else if(c == '3')
		{
			if(Walking::GetInstance()->IsRunning() == false )
			{
				fprintf(stderr, "STARTING\n");
				Action::GetInstance()->Start(9);
				while(Action::GetInstance()->IsRunning()) usleep(8*1000);
				//Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
				Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true,true);
				Walking::GetInstance()->X_MOVE_AMPLITUDE = 0;
				Walking::GetInstance()->Y_MOVE_AMPLITUDE = 0;
				Walking::GetInstance()->A_MOVE_AMPLITUDE = 0;
				Walking::GetInstance()->Start();
			}
			else
				Walking::GetInstance()->A_MOVE_AMPLITUDE = 0;
		}
		else if(c == '4')
		{
			if(Walking::GetInstance()->IsRunning() == true )
			{
				fprintf(stderr, "TURNING RIGHT\n");
				Walking::GetInstance()->A_MOVE_AMPLITUDE = -5;

			}
		}
		else if(c == '5')
		{
			if(Walking::GetInstance()->IsRunning() == true )
			{
				fprintf(stderr, "TURNING LEFT\n");
				Walking::GetInstance()->A_MOVE_AMPLITUDE = 5;
			}
		}
		else if(c == '6')
		{
			if(Walking::GetInstance()->IsRunning() == true )
			{
				fprintf(stderr, "STOPPING\n");		
				Walking::GetInstance()->Stop();
				while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);
				Walking::GetInstance()->m_Joint.SetEnableBody(false);
				Head::GetInstance()->m_Joint.SetEnableBody(false);
				Action::GetInstance()->m_Joint.SetEnableBody(true);
				MotionManager::GetInstance()->SetEnable(true);
			}
		}
		else if(c == '7')
		{
			for(int i=JointData::ID_R_SHOULDER_PITCH; i<JointData::NUMBER_OF_JOINTS; i++)
											Reset(&cm730, i);

									Reset(&cm730, CM730::ID_CM);
		}
		else if(c == '8')
		{

			float tilt;
			tilt = GetTilt();
			printf("%f\n", tilt);

		}
		else if(c == '9')
		{
			float tilt,pan;
			cout<<"enter tilt\n";
			cin>>tilt;
			cout<<"enter pan\n";
			cin>>pan;
			SetTilt(pan,tilt);
		}
		else if(c == 'q')	
int main(void)
{
    //Register signal and signal handler
    signal(SIGINT, signal_callback_handler);

    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 ////////////////////////////
    if (MotionManager::GetInstance()->Initialize(&arbotixpro) == false)
        {
            linux_arbotixpro.SetPortName(U2D_DEV_NAME1);
            if (MotionManager::GetInstance()->Initialize(&arbotixpro) == 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();

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

    while (isRunning)
        {
            //usleep(10000);
            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);
        }

    return 0;
}
int main(void)
{
    signal(SIGABRT, &sighandler);
    signal(SIGTERM, &sighandler);
    signal(SIGQUIT, &sighandler);
    signal(SIGINT, &sighandler);

    change_current_dir();

    minIni* ini = new minIni(INI_FILE_PATH);
    Image* rgb_output = new Image(Camera::WIDTH, Camera::HEIGHT, Image::RGB_PIXEL_SIZE);

    LinuxCamera::GetInstance()->Initialize(0);
    LinuxCamera::GetInstance()->SetCameraSettings(CameraSettings());    // set default
    LinuxCamera::GetInstance()->LoadINISettings(ini);                   // load from 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();
    BallFollower follower = BallFollower();

    ColorFinder* red_finder = new ColorFinder(0, 15, 45, 0, 0.3, 50.0);
    red_finder->LoadINISettings(ini, "RED");
    httpd::red_finder = red_finder;

    ColorFinder* yellow_finder = new ColorFinder(60, 15, 45, 0, 0.3, 50.0);
    yellow_finder->LoadINISettings(ini, "YELLOW");
    httpd::yellow_finder = yellow_finder;

    ColorFinder* blue_finder = new ColorFinder(225, 15, 45, 0, 0.3, 50.0);
    blue_finder->LoadINISettings(ini, "BLUE");
    httpd::blue_finder = blue_finder;

    httpd::ini = ini;

    //////////////////// 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);

    MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance());
    MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
    MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance());

    LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance());
    motion_timer->Start();
    /////////////////////////////////////////////////////////////////////
    
    MotionManager::GetInstance()->LoadINISettings(ini);

    int firm_ver = 0;
    if(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);
        exit(0);
    }

    if(0 < firm_ver && firm_ver < 27)
    {
#ifdef MX28_1024
        Action::GetInstance()->LoadFile(MOTION_FILE_PATH);
#else
        fprintf(stderr, "MX-28's firmware is not support 4096 resolution!! \n");
        fprintf(stderr, "Upgrade MX-28's firmware to version 27(0x1B) or higher.\n\n");
        exit(0);
#endif
    }
    else if(27 <= firm_ver)
    {
#ifdef MX28_1024
        fprintf(stderr, "MX-28's firmware is not support 1024 resolution!! \n");
        fprintf(stderr, "Remove '#define MX28_1024' from 'MX28.h' file and rebuild.\n\n");
        exit(0);
#else
        Action::GetInstance()->LoadFile((char*)MOTION_FILE_PATH);
#endif
    }
    else
        exit(0);

    Action::GetInstance()->m_Joint.SetEnableBody(true, true);
    MotionManager::GetInstance()->SetEnable(true);

    cm730.WriteByte(CM730::P_LED_PANNEL, 0x01|0x02|0x04, NULL);

    LinuxActionScript::PlayMP3("../../../Data/mp3/Demonstration ready mode.mp3");
    Action::GetInstance()->Start(15);
    while(Action::GetInstance()->IsRunning()) usleep(8*1000);

    while(1)
    {
        StatusCheck::Check(cm730);

        Point2D ball_pos, red_pos, yellow_pos, blue_pos;

        LinuxCamera::GetInstance()->CaptureFrame();
        memcpy(rgb_output->m_ImageData, LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_ImageData, LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_ImageSize);

        if(StatusCheck::m_cur_mode == READY || StatusCheck::m_cur_mode == VISION)
        {
            ball_pos = ball_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
            red_pos = red_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
            yellow_pos = yellow_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
            blue_pos = blue_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);

            unsigned char r, g, b;
            for(int i = 0; i < rgb_output->m_NumberOfPixels; i++)
            {
                r = 0; g = 0; b = 0;
                if(ball_finder->m_result->m_ImageData[i] == 1)
                {
                    r = 255;
                    g = 128;
                    b = 0;
                }
                if(red_finder->m_result->m_ImageData[i] == 1)
                {
                    if(ball_finder->m_result->m_ImageData[i] == 1)
                    {
                        r = 0;
                        g = 255;
                        b = 0;
                    }
                    else
                    {
                        r = 255;
                        g = 0;
                        b = 0;
                    }
                }
                if(yellow_finder->m_result->m_ImageData[i] == 1)
                {
                    if(ball_finder->m_result->m_ImageData[i] == 1)
                    {
                        r = 0;
                        g = 255;
                        b = 0;
                    }
                    else
                    {
                        r = 255;
                        g = 255;
                        b = 0;
                    }
                }
                if(blue_finder->m_result->m_ImageData[i] == 1)
                {
                    if(ball_finder->m_result->m_ImageData[i] == 1)
                    {
                        r = 0;
                        g = 255;
                        b = 0;
                    }
                    else
                    {
                        r = 0;
                        g = 0;
                        b = 255;
                    }
                }

                if(r > 0 || g > 0 || b > 0)
                {
                    rgb_output->m_ImageData[i * rgb_output->m_PixelSize + 0] = r;
                    rgb_output->m_ImageData[i * rgb_output->m_PixelSize + 1] = g;
                    rgb_output->m_ImageData[i * rgb_output->m_PixelSize + 2] = b;
                }
            }
        }
        else if(StatusCheck::m_cur_mode == SOCCER)
        {
            tracker.Process(ball_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame));

            for(int i = 0; i < rgb_output->m_NumberOfPixels; i++)
            {
                if(ball_finder->m_result->m_ImageData[i] == 1)
                {
                    rgb_output->m_ImageData[i*rgb_output->m_PixelSize + 0] = 255;
                    rgb_output->m_ImageData[i*rgb_output->m_PixelSize + 1] = 128;
                    rgb_output->m_ImageData[i*rgb_output->m_PixelSize + 2] = 0;
                }
            }
        }

        streamer->send_image(rgb_output);

        if(StatusCheck::m_is_started == 0)
            continue;

        switch(StatusCheck::m_cur_mode)
        {
        case READY:
            break;
        case SOCCER:
            if(Action::GetInstance()->IsRunning() == 0)
            {
                Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
                Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);

                follower.Process(tracker.ball_position);

                if(follower.KickBall != 0)
                {
                    Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
                    Action::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);

                    if(follower.KickBall == -1)
                    {
                        Action::GetInstance()->Start(12);   // RIGHT KICK
                        fprintf(stderr, "RightKick! \n");
                    }
                    else if(follower.KickBall == 1)
                    {
                        Action::GetInstance()->Start(13);   // LEFT KICK
                        fprintf(stderr, "LeftKick! \n");
                    }
                }
            }
            break;
        case MOTION:
            if(LinuxActionScript::m_is_running == 0)
                LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH);
            break;
        case VISION:
            int detected_color = 0;
            detected_color |= (red_pos.X == -1)? 0 : VisionMode::RED;
            detected_color |= (yellow_pos.X == -1)? 0 : VisionMode::YELLOW;
            detected_color |= (blue_pos.X == -1)? 0 : VisionMode::BLUE;

            if(Action::GetInstance()->IsRunning() == 0)
                VisionMode::Play(detected_color);
            break;
        }
    }

    return 0;
}
Exemple #9
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;
}
Exemple #10
0
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;
}