//! @brief Returns camera settings.
CameraSettings DataWrapper::getCameraSettings() const
{
return CameraSettings();
}
void initFlyBys () {
std::list<CameraSettings> flyBy;
flyBy.push_back(CameraSettings(0, Vector3(-732.393, 370, 665.959), Vector3(0, 0, 0), Vector3(0, 1, 0), EASE_ELASTICIN));
flyBy.push_back(CameraSettings(100, Vector3(2.63524, 150, 142.426), Vector3(0, 0, 0), Vector3(0, 1, 0), EASE_ELASTICIN));
flyBy.push_back(CameraSettings(200, Vector3(513.977, 370, 846.008), Vector3(0, 0, 0), Vector3(0, 1, 0), EASE_ELASTICIN));
flyBy.push_back(CameraSettings(300, Vector3(-732.393, 370, 665.959), Vector3(0, 0, 0), Vector3(0, 1, 0), EASE_ELASTICIN));
std::list<CameraSettings> flyBy2;
for (int i = 0; i <= 360; i+=10) {
flyBy2.push_back(CameraSettings(i, Vector3(500.0 * cos(i * (M_PI / 180)), 200.0 + 50 * cos((i * 2) * (M_PI / 180)), 500.0 * sin(i * (M_PI / 180))), Vector3(0, 0, 0), Vector3(0.0, 1.0, 0.0), EASE_LINEAR));
}
std::list<CameraSettings> flyBy3;
/*
Camera position: Vector3(-2499.3, 61.151, 716.848)
Camera lookat: Vector3(-2498.3, 61.1615, 716.879)
Camera up: Vector3(0, 1, 0)
*/
flyBy3.push_back(CameraSettings(0, Vector3(-2499.3, 61.151, 716.848), Vector3(-2498.3, 61.1615, 716.879), Vector3(0, 1, 0), EASE_LINEAR));
/*
Camera position: Vector3(-1927.46, 56.2293, 892.343)
Camera lookat: Vector3(-1926.53, 56.2188, 891.983)
Camera up: Vector3(-0.819152, 0.573576, 0)
*/
flyBy3.push_back(CameraSettings(100, Vector3(-1927.46, 56.2293, 892.343), Vector3(-1926.53, 56.2188, 891.983), Vector3(-0.819152, 0.573576, 0), EASE_LINEAR));
/*
Camera position: Vector3(-1196.03, 70.8737, 884.67)
Camera lookat: Vector3(-1195.24, 70.947, 884.065)
Camera up: Vector3(-0.939693, 0.34202, 0)
*/
flyBy3.push_back(CameraSettings(200, Vector3(-1196.03, 70.8737, 884.67), Vector3(-1195.24, 70.947, 884.065), Vector3(-0.939693, 0.34202, 0), EASE_LINEAR));
/*
Camera position: Vector3(-430.245, 78.7308, 833.739)
Camera lookat: Vector3(-429.759, 78.8353, 832.871)
Camera up: Vector3(-0.34202, 0.939693, 0)
*/
flyBy3.push_back(CameraSettings(300, Vector3(-430.245, 78.7308, 833.739), Vector3(-429.759, 78.8353, 832.871), Vector3(-0.34202, 0.939693, 0), EASE_LINEAR));
/*
Camera position: Vector3(8.12899, 6.44424, 108.208)
Camera lookat: Vector3(8.09764, 6.50703, 107.21)
Camera up: Vector3(0, 1, 0)
*/
flyBy3.push_back(CameraSettings(400, Vector3(8.12899, 6.44424, 108.208), Vector3(8.09764, 6.50703, 107.21), Vector3(0, 1, 0), EASE_LINEAR));
flyBy3.push_back(CameraSettings(500, Vector3(8.12899, 6.44424, 108.208), Vector3(8.09764, 6.50703, 107.21), Vector3(0, 1, 0), EASE_LINEAR));
flyBys.push_back(flyBy);
flyBys.push_back(flyBy2);
flyBys.push_back(flyBy3);
}
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;
}
