void StrColRoSeService::execute() {
mBBThread.reset(new boost::thread(boost::bind(&StrColRoSeService::blackboardReadRun, this)));
Planes detectedPlanes;
Cylinders detectedCylinders;
while (isRunning()&&ros::ok()) {
RoSe::Mutex::AutoLocker lock(mMutex);
mCondition.wait();
if (mLastPointCloud && isRunning() && ros::ok()) {
detectedPlanes.clear();
detectedCylinders.clear();
//segment pointCloud mLastPointCloud
PlanePtrs pointMapping(mLastPointCloud->points.size());
CylinderPtrs cylinderMapping;
mStrCol.doSegmentation(mLastPointCloud, pointMapping, detectedPlanes, cylinderMapping, detectedCylinders);
//send unsegmented points
RosePointcloudPtr unsegmentedPoints(new RosePointcloud);
extractUnmappedPointsToRosePC(unsegmentedPoints, mLastPointCloud, pointMapping);
sendPoints(unsegmentedPoints);
std::cout << unsegmentedPoints->points().size() << " points were not segmented and sent back" << std::endl;
//send detected planes
sendPlanes(detectedPlanes);
mLastPointCloud.reset();
}
}
mBBThread->join();
}
int companyFunc(char * fileName, int companyID) {
FILE * file = NULL;
int i, j;
struct sigaction signalAction;
signalAction.sa_flags = 0;
signalAction.sa_handler = sigintServHandler;
sigfillset(&(signalAction.sa_mask));
sigaction(SIGINT, &signalAction, NULL);
if( (client = connectToServer(server)) == NULL )
{
fprintf(stderr, "Company %d couldn't connect to server.", companyID);
return 1;
}
if( (clients[0] = getClient(server, getppid())) == NULL )
{
fprintf(stderr, "Map client not found\n");
return 1;
}
sendChecksign(clients[0]);
if (!openFile(&file, fileName)){
if (createCompany(file, &compa)) {
fprintf(stderr, "File Error\n");
return 1;
}
fclose(file);
} else {
fprintf(stderr, "Impossible to open file\n");
return 1;
}
compa->ID = companyID;
for (i = 0; i < compa->planesCount; i++)
if ((compa->companyPlanes[i]->destinationID = getCityID(compa->companyPlanes[i]->startCity, mapSt)) == -1) {
fprintf(stderr, "Invalid start city for plane %d in company %d.\n", i, compa->ID);
return 1;
}
if( (planes = malloc(sizeof(plane *) * compa->planesCount)) == NULL)
return 1;
if( (threads = malloc(sizeof(pthread_t *) * compa->planesCount)) == NULL)
return 1;
pthread_mutex_init(&mutexVar, NULL);
pthread_cond_init(&condVar, NULL);
pthread_mutex_init(&planeMutex, NULL);
pthread_cond_init(&planeCond, NULL);
pthread_mutex_lock(&mutexVar); /*create planes and wait for them to be initialized*/
planesChecked = 0;
for(i = 0; i < compa->planesCount; i++)
pthread_create(&threads[i], NULL, threadFunc, (void*)i);
while(planesChecked < compa->planesCount)
pthread_cond_wait(&condVar, &mutexVar);
pthread_mutex_unlock(&mutexVar);
while (TRUE) {
unloading = 0;
rcvMap(&med, client, mapSt->citiesCount);
pthread_mutex_lock(&planeMutex); /*wake up planes and wait until they check cargo*/
pthread_mutex_lock(&mutexVar);
planesChecked = 0;
pthread_cond_broadcast(&planeCond);
pthread_mutex_unlock(&planeMutex);
while(planesChecked < compa->planesCount)
pthread_cond_wait(&condVar, &mutexVar);
pthread_mutex_unlock(&mutexVar);
for (i = 0; i < mapSt->citiesCount; i++) {
for (j = 0; med[i][j] != NULL; j++) {
free(med[i][j]->name);
free(med[i][j]);
}
free(med[i]);
}
free(med);
if (!unloading)
sendPlanes(compa->ID, unloading, NULL, clients[0]);
else {
sendPlanes(compa->ID, unloading, planes, clients[0]);
rcvPlanes(NULL, &unloading, &receivedPlanes, client);
}
pthread_mutex_lock(&planeMutex); /*wake up planes and wait until they update cargo*/
pthread_mutex_lock(&mutexVar);
planesChecked = 0;
pthread_cond_broadcast(&planeCond);
pthread_mutex_unlock(&planeMutex);
while(planesChecked < compa->planesCount)
pthread_cond_wait(&condVar, &mutexVar);
pthread_mutex_unlock(&mutexVar);
if(unloading)
{
for (i = 0; i < unloading; i++) {
for (j = 0; j < receivedPlanes[i]->medCount; j++) {
free(receivedPlanes[i]->medicines[j]->name);
free(receivedPlanes[i]->medicines[j]);
}
free(receivedPlanes[i]->medicines);
free(receivedPlanes[i]);
}
free(receivedPlanes);
}
rcvMap(&med, client, mapSt->citiesCount);
pthread_mutex_lock(&planeMutex); /*wake up planes and wait until they set new destination*/
pthread_mutex_lock(&mutexVar);
planesChecked = 0;
pthread_cond_broadcast(&planeCond);
pthread_mutex_unlock(&planeMutex);
while(planesChecked < compa->planesCount)
pthread_cond_wait(&condVar, &mutexVar);
pthread_mutex_unlock(&mutexVar);
for (i = 0; i < mapSt->citiesCount; i++) {
for (j = 0; med[i][j] != NULL; j++) {
free(med[i][j]->name);
free(med[i][j]);
}
free(med[i]);
}
free(med);
}
return 1;
}
