//boucle de traitement de la kinect
void* Kinect::runKinect(void* k)
{
//setup
Kinect* kinect = static_cast<Kinect*>(k);
drawgrid = kinect->grille_;
Mat depthMat(480,640,CV_16UC1);
Freenect::Freenect freenect;
MyFreenectDevice& device = freenect.createDevice<MyFreenectDevice>(0);
device.startDepth();
//grille ¨virtuelle¨ plus petite pour A* au forum (pour accélérer la recherche de trajectoire).
char **sgrille = new char*[175];
for(int i=0;i<175;i++)
sgrille[i]=new char[175];
clock_t time;
while (!kinect->die_)
{
//si pas d´image disponible : attente de 10ms puis relancer la boucle.
if(!device.getDepth(depthMat))
{
delay(10);
continue;
}
pthread_mutex_lock(&mut);
depthMat.convertTo(depthf, CV_8UC1, 255.0/2048.0);
time = clock(); //debug : utilisé pour cronométrer les boucles
kinect->marquer_obstacles(depthMat);
// std::cout << "marquer" << (clock() - time) * 1000 / CLOCKS_PER_SEC << std::endl;
//executer AStar, division de grille par 2 pour aller plus vite
for(int x =0; x <175; x++)
for(int y =0; y <175; y++)
sgrille[x][y] = kinect->grille_->grille_[2 *x][2 * y] + kinect->grille_->grille_[2 *x + 1][2 * y] + kinect->grille_->grille_[2 *x][2 * y + 1] + kinect->grille_->grille_[2 *x+1][2 * y+1];
char *route = AStarStatic(240 / 2, 0, 240 / 2, TARGET / 2, sgrille, 175,175);
char lroute[ 2 * strlen(route)];
for(int x =0; x <strlen(route); x++)
{
lroute[2 * x] = route[x];
lroute[2 * x + 1] = route[x];
}
drawRouteOnMap(kinect->grille_->grille_, lroute, 240, 0);
if(strlen(route) != 0)
free(route);
// std::cout << (clock() - time) * 1000 / CLOCKS_PER_SEC << std::endl;
pthread_cond_signal(&cond);
pthread_mutex_unlock(&mut);
delay(40); //40ms sleep : laisse le temps pour dessiner la fenetre avant de reprendre le mutex
}
device.stopDepth();
delete(sgrille);
return 0;
}
