void *loadConsume(void *d) {
void *result = NULL;
pthread_mutex_lock(&mainHeap_x);
if (d != NULL) {
Load *l = (Load *)d;
unsigned int *t = (unsigned int *)l->data;
printf("t: %d\n", *t);
char *msg = malloc(sizeof(char) * (1 + *t));
int i;
for (i=0; i < (*t + 1); ++i) {
msg[i] = 'a' + i;
}
msg[i] = '\0';
result = msg;
l->id = getAvailableId(l->srcHeap);
printf("loadId: %d %p\n", l->id, l);
heapifyFromHead(l->srcHeap);
#ifdef DEBUG
printf("\033[96mnew heapify ");
printHeap(l->srcHeap, printLoad);
#endif
pthread_cond_broadcast(&mainHeap_dt);
}
pthread_mutex_unlock(&mainHeap_x);
return result;
}
void FaceDetectorFilter::facesCallback(const pcl::PointCloud<pcl::PointXYZL>::ConstPtr& msg)
{
tf::Transform cameraTransform;
try {
tf::StampedTransform tr;
transformListener.lookupTransform ( "odom","camera_link2",ros::Time(0),tr);
cameraTransform=tr;
} catch(...) {
return;
}
std::set<unsigned int> usedUsers = deleteOld();
std::list<Point> incomingUsers;
fillList(incomingUsers, msg,cameraTransform);
while(1) {
std::pair<unsigned int,std::list<Point>::iterator> match = findClosest(incomingUsers);
if(match.first == 0)
break;
float distance = getDistance(users[match.first],*match.second);
if(distance> MAX_SPEED)
break;
if(usedUsers.find(match.first) == usedUsers.end()) {
users[match.first] = *match.second;
usedUsers.insert(match.first);
std::cerr<<"user updated: "<<match.first<<", distance:" <<distance<<std::endl;
} else {
std::cerr<<"user ignored: "<<match.first<<", distance:" <<distance<<std::endl;
}
incomingUsers.erase(match.second);
}
for(std::list<Point>::iterator it = incomingUsers.begin(); it!=incomingUsers.end(); ++it) {
unsigned int newId = getAvailableId(usedUsers);
users[newId] = *it;
std::cerr<<"added user: "******"camera_link2";
pmsg->height = 1;
for(std::map<unsigned int,Point>::iterator it = users.begin(); it != users.end(); ++it) {
pcl::PointXYZL point;
Point p(it->second);
transformPoint(p,cameraTransform,true);
point.label = it->first;
point.x=p.x;
point.y=p.y;
point.z = p.z;
pmsg->points.push_back(point);
}
pmsg->width = pmsg->points.size();
facePublisher.publish(pmsg);
}
void produce(unsigned int n, void *(*func)(void *)) {
int maxHandle = 8, readyId=-1, nC = 0;
Heap *wHeap = NULL, *restHeap = NULL;
wHeap = initHeap(wHeap, loadComp, freeLoad);
restHeap = initHeap(restHeap, loadComp, freeLoad);
unsigned int minThreshold = maxHandle > n ? n : maxHandle;
pthread_t thList[minThreshold];
pthread_attr_t attr;
pthread_attr_init(&attr);
#ifdef __linux__
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
#endif
for (readyId=0; readyId < minThreshold; ++readyId) {
unsigned int *iNew = (unsigned int *)malloc(sizeof(unsigned int));
*iNew = readyId;
Load *l = createLoad(iNew);
l->thId = readyId;
addLoad(wHeap, l);
pthread_create(thList + readyId, &attr, func, l);
}
int i, minFreeLoadCount = minThreshold >> 1;
if (readyId < n) {
heapifyFromHead(wHeap);
i = readyId;
int chillThId = -1;
Load *wHead = NULL;
while (i < n) {
printf("\033[32mwHeap: "); printHeap(wHeap, printLoad);
printf("\n\033[33mrHeap: "); printHeap(restHeap, printLoad);
printf("\ni:%d n:%d chillThId: %d\033[00m\n", i, n, chillThId);
if ((wHead = peek(wHeap)) != NULL) {
printf("wHead: %d\n", wHead->id);
void *data = NULL;
int join = pthread_join(thList[wHead->thId], &data);
printf("newJoin: %d\n", join);
if (! join) {
printf("joined: %d\n", wHead->thId);
if (data != NULL) {
printf("\033[36m\nRetr %s :%d\033[00m\n", (char *)data, wHead->thId);
free(data);
}
chillThId = wHead->thId;
printf("chillThId: %d\n", chillThId);
#ifdef DEBUG
printf("wHead->thId: %d\n", wHead->thId);
#endif
heapExtract(wHeap, (const void **)&wHead);
wHead->id = getAvailableId(restHeap);
addLoad(restHeap, wHead);
printf("rHeap"); printHeap(restHeap, printLoad);
wHead = NULL;
}
}
if (getSize(wHeap) < minFreeLoadCount && peek(restHeap) != NULL) {
#ifdef DEBUG
printf("Peeked: %p\n", peek(restHeap));
printf("\nrestHeap\n");
#endif
heapExtract(restHeap, (const void **)&wHead);
if (wHead == NULL) continue;
#ifdef DEBUG
printf("wHead->thId:: %p\n", wHead);
#endif
wHead->thId = chillThId;
*((int *)wHead->data) = i;
addLoad(wHeap, wHead);
int createStatus =\
pthread_create(thList + wHead->thId, &attr, func, wHead);
printf("createdStatus: %d i: %d\n", createStatus, i);
if (! createStatus) {
++i;
}
}
}
}
while (! isEmpty(wHeap)) {
Load *tmpHead = NULL;
if (! heapExtract(wHeap, (const void **)&tmpHead) && tmpHead != NULL) {
void *data = NULL;
if (! pthread_join(thList[tmpHead->thId], &data)) {
if (data != NULL) {
printf("i: %d Joined msg: %s\n", i, (char *)data);
free(data);
}
}
}
freeLoad(tmpHead);
}
destroyHeap(wHeap);
destroyHeap(restHeap);
}
// get request from client socket
void parseClientRequest(player p, char * buffer) {
// socket closed
if(readline(p->fd, buffer, MAXBUF-1) == 0)
removeClient(p);
else if(strlen(buffer) > 3 && buffer[3] == '#') { // new message
#ifdef DEBUG
printf("\r[DEBUG] : %s\n", buffer);
#endif
buffer[3] = 0;
// broadcast message
if(strcmp(buffer, CMD_MESSAGE) == 0)
broadcastMessage(p, buffer);
// broadcast action
else if(strcmp(buffer, CMD_ACTION) == 0)
broadcastMessage(p, buffer);
// bind to a game
else if(strlen(buffer+4) > 6 && buffer[9] == '#' && strcmp(buffer, CMD_BIND) == 0)
bindToGame(p, buffer);
// create a game
else if(strlen(buffer+4) > 6 && buffer[9] == '#' && strcmp(buffer, CMD_CREATE_GAME) == 0)
createGame(p, buffer);
// list existing games
else if(strcmp(buffer, CMD_LIST) == 0)
listGames(p);
// Get available id to creage a game
else if(strcmp(buffer, CMD_CREATE) == 0)
getAvailableId(p);
// Create new socket to send a file
else if(strlen(buffer+4) > 7 && buffer[10] == '#' && strcmp(buffer, CMD_FILE) == 0)
transfer_file(p, buffer);
// Register as a Game Master
else if(strcmp(buffer, CMD_REGISTER_GM) == 0)
register_game_master(p);
// Send a message to game master
else if(strcmp(buffer, CMD_SEND_GM) == 0)
send_to_game_master(p, buffer);
// Send a message to a specific player
else if(strcmp(buffer, CMD_SEND_TO_PLAYER) == 0)
send_to_player(p, buffer);
else
senderror(p->fd, NULL, ERR_NOT_RECOGNIZED);
}
else {
#ifdef DEBUG
printf("\r[DEBUG] : %s\n", buffer);
#endif
senderror(p->fd, NULL, ERR_NOT_RECOGNIZED);
}
}
