int main (int argc, char* argv[])
{
//create an array of processes called queue.
ticks = 0;
loop = 0;
queue q;
createQueue(q);
while (loop == 0)
{
ticks++;
printf("%d",ticks);
int i;
for (i = 1; i < getQueueSize(q); i++)
{
if (ticks - q.items[i].timeStamp > ticks)
{
if (getQueueSize(q) > 0)
{
scheduleProcess(q);
}
}
}
getUserInput(q);
if (ticks % 10 == 0)
{
for (i = 1; i < getQueueSize(q); i++)
{
q.items[i].priority = q.items[i].priority + 1;
}
}
}
}
void BFSTaskP2P::open(Graph *newGraph)
{
SearchTask::open(newGraph);
queue = new Vertex[getQueueSize()];
nextQueue = new Vertex[getQueueSize()];
parent = new Vertex[numLocalVertex];
}
const ParScannedTokenQueue::scannedTokenInfo &
ParScannedTokenQueue::getScannedTokenInfo(const Int32 tokenInfoIndex) const
{
ComASSERT(tokenInfoIndex <= 0 AND
getQueueSize() > - tokenInfoIndex);
return scannedTokens_[(currentPos_ + getQueueSize()
+ tokenInfoIndex) % getQueueSize()];
}
void BFSTaskRMAFetch::open(Graph *newGraph)
{
SearchTask::open(newGraph);
qWin = new RMAWindow<Vertex>(comm, getQueueSize(), VERTEX_TYPE);
nextQWin = new RMAWindow<Vertex>(comm, getQueueSize(), VERTEX_TYPE);
pWin = new RMAWindow<Vertex>(comm, numLocalVertex, VERTEX_TYPE);
queue = qWin->getData();
nextQueue = nextQWin->getData();
parent = pWin->getData();
}
static void
cancelOperation (Element *operationElement) {
OperationEntry *operation = getElementItem(operationElement);
if (operation->active) {
operation->cancel = 1;
} else {
FunctionEntry *function = operation->function;
int isFirstOperation = operationElement == getActiveOperationElement(function);
if (isFirstOperation) {
if (!operation->finished) {
if (operation->function->methods->cancelOperation) {
operation->function->methods->cancelOperation(operation);
}
}
}
if (getQueueSize(function->operations) == 1) {
deleteElement(findElementWithItem(getFunctionQueue(0), function));
} else {
deleteElement(operationElement);
if (isFirstOperation) {
operationElement = getActiveOperationElement(function);
operation = getElementItem(operationElement);
if (!operation->finished) startOperation(operation);
}
}
}
}
Depth::Depth(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private)
: nh_(nh),
nh_private_(nh_private),
it_(nh_),
queue_size_(getQueueSize()),
first_image_sub_(it_, "rect/first/image", queue_size_),
second_image_sub_(it_, "rect/second/image", queue_size_),
first_camera_info_sub_(nh_, "rect/first/camera_info", queue_size_),
second_camera_info_sub_(nh_, "rect/second/camera_info", queue_size_),
camera_sync_(CameraSyncPolicy(queue_size_), first_image_sub_,
second_image_sub_, first_camera_info_sub_,
second_camera_info_sub_) {
std::string pre_filter_type_string;
nh_private_.param("pre_filter_type", pre_filter_type_string, kPreFilterType);
if (pre_filter_type_string == std::string("xsobel")) {
pre_filter_type_ = cv::StereoBM::PREFILTER_XSOBEL;
} else if (pre_filter_type_string == std::string("normalized_response")) {
pre_filter_type_ = cv::StereoBM::PREFILTER_NORMALIZED_RESPONSE;
} else {
throw std::runtime_error(
"Unrecognized prefilter type, choices are 'xsobel' or "
"'normalized_response'");
}
// general stereo parameters
nh_private_.param("min_disparity", min_disparity_, kMinDisparity);
nh_private_.param("num_disparities", num_disparities_, kNumDisparities);
nh_private_.param("pre_filter_cap", pre_filter_cap_, kPreFilterCap);
nh_private_.param("uniqueness_ratio", uniqueness_ratio_, kUniquenessRatio);
nh_private_.param("speckle_range", speckle_range_, kSpeckleRange);
nh_private_.param("speckle_window_size", speckle_window_size_,
kSpeckleWindowSize);
nh_private_.param("sad_window_size", sad_window_size_, kSADWindowSize);
// bm parameters
nh_private_.param("texture_threshold", texture_threshold_, kTextureThreshold);
nh_private_.param("pre_filter_size", pre_filter_size_, kPreFilterSize);
// sgbm parameters
nh_private_.param("use_sgbm", use_sgbm_, kUseSGBM);
nh_private_.param("p1", p1_, kP1);
nh_private_.param("p2", p2_, kP2);
nh_private_.param("disp_12_max_diff", disp_12_max_diff_, kDisp12MaxDiff);
nh_private_.param("use_mode_HH", use_mode_HH_, kUseHHMode);
nh_private_.param("do_median_blur", do_median_blur_, kDoMedianBlur);
camera_sync_.registerCallback(
boost::bind(&Depth::camerasCallback, this, _1, _2, _3, _4));
disparity_pub_ = it_.advertise("disparity/image", queue_size_);
pointcloud_pub_ =
nh_.advertise<sensor_msgs::PointCloud2>("pointcloud", queue_size_);
freespace_pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(
"freespace_pointcloud", queue_size_);
}
IOMemoryDescriptor *IOSharedDataQueue::getMemoryDescriptor()
{
IOMemoryDescriptor *descriptor = 0;
if (dataQueue != 0) {
descriptor = IOMemoryDescriptor::withAddress(dataQueue, getQueueSize() + DATA_QUEUE_MEMORY_HEADER_SIZE + DATA_QUEUE_MEMORY_APPENDIX_SIZE, kIODirectionOutIn);
}
return descriptor;
}
ParScannedTokenQueue::ParScannedTokenQueue()
: currentPos_(-1)
{
for (Int32 i = 0; i < getQueueSize(); i++)
{
scannedTokens_[i].tokenStrPos = 0;
scannedTokens_[i].tokenStrLen = 0;
scannedTokens_[i].tokenStrInputLen = 0;
scannedTokens_[i].tokenStrOffset = 0;
scannedTokens_[i].tokenIsComment = FALSE;
}
}
//Inputs: queue
//Outputs: none
//Purpose: Prints the queue
void printQueue(queue q)
{
int i;
for (i = 0; i < getQueueSize(q); i++)
{
printf("Queue Entry %d",i);
printf(" id: %d", q.items[i].id);
printf(" timeStamp: %d", q.items[i].timeStamp);
printf(" duration: %d", q.items[i].duration);
printf(" priority: %d\n", q.items[i].priority);
}
return;
}
void
ParScannedTokenQueue::insert(const size_t tokenStrPos,
const size_t tokenStrLen,
const size_t tokenStrOffset,
NABoolean tokenIsComment)
{
currentPos_ = (currentPos_ + 1) % getQueueSize();
scannedTokenInfo & tokInfo = scannedTokens_[currentPos_];
tokInfo.tokenStrPos = tokenStrPos + tokenStrOffset;
tokInfo.tokenStrLen = tokenStrLen;
tokInfo.tokenStrInputLen = tokenStrLen;
tokInfo.tokenStrOffset = tokenStrOffset;
tokInfo.tokenIsComment = tokenIsComment;
}
void
destroyKeyboardMonitorObject (KeyboardMonitorObject *kmo) {
kmo->isActive = 0;
while (getQueueSize(kmo->instanceQueue) > 0) {
Element *element = getQueueHead(kmo->instanceQueue);
KeyboardInstanceObject *kio = getElementItem(element);
destroyKeyboardInstanceObject(kio);
}
if (kmo->instanceQueue) deallocateQueue(kmo->instanceQueue);
if (kmo->kmx) destroyKeyboardMonitorExtension(kmo->kmx);
free(kmo);
}
recvQueue* addToQueue(recvQueue* head, packet* p) {
if (getQueueSize(head) == 0) {
head->data = p;
return head;
}
recvQueue* copy = head;
while (copy->next) {
copy = copy->next;
}
copy->next = initRecvQueue();
copy = copy->next;
copy->data = p;
return head;
}
void Worker::run(HttpHandler handler) {
this->httpHandler = handler;
while(true) {
{
std::unique_lock<std::mutex> locker(waiting_lock);
while(!notified) { // от ложных пробуждений
waiting_client.wait(locker);
}
notified = false;
}
while (getQueueSize()) {
httpHandler.exec(nextClient());
}
}
}
void IOHIDEventQueue::start()
{
if ( _lock )
IOLockLock(_lock);
if ( _state & kHIDQueueStarted )
goto START_END;
if ( _currentEntrySize != _maxEntrySize )
{
mach_port_t port = notifyMsg ? ((mach_msg_header_t *)notifyMsg)->msgh_remote_port : MACH_PORT_NULL;
// Free the existing queue data
if (dataQueue) {
IOFreeAligned(dataQueue, round_page_32(getQueueSize() + DATA_QUEUE_MEMORY_HEADER_SIZE));
dataQueue = NULL;
}
if (_descriptor) {
_descriptor->release();
_descriptor = 0;
}
// init the queue again. This will allocate the appropriate data.
if ( !initWithEntries(_numEntries, _maxEntrySize) ) {
goto START_END;
}
_currentEntrySize = _maxEntrySize;
// RY: since we are initing the queue, we should reset the port as well
if ( port )
setNotificationPort(port);
}
else if ( dataQueue )
{
dataQueue->head = 0;
dataQueue->tail = 0;
}
_state |= kHIDQueueStarted;
START_END:
if ( _lock )
IOLockUnlock(_lock);
}
IODataQueueEntry * IOSharedDataQueue::peek()
{
IODataQueueEntry *entry = 0;
UInt32 headOffset;
UInt32 tailOffset;
if (!dataQueue) {
return NULL;
}
// Read head and tail with acquire barrier
// See rdar://problem/40780584 for an explanation of relaxed/acquire barriers
headOffset = __c11_atomic_load((_Atomic UInt32 *)&dataQueue->head, __ATOMIC_RELAXED);
tailOffset = __c11_atomic_load((_Atomic UInt32 *)&dataQueue->tail, __ATOMIC_ACQUIRE);
if (headOffset != tailOffset) {
IODataQueueEntry * head = 0;
UInt32 headSize = 0;
UInt32 headOffset = dataQueue->head;
UInt32 queueSize = getQueueSize();
if (headOffset >= queueSize) {
return NULL;
}
head = (IODataQueueEntry *)((char *)dataQueue->queue + headOffset);
headSize = head->size;
// Check if there's enough room before the end of the queue for a header.
// If there is room, check if there's enough room to hold the header and
// the data.
if ((headOffset > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) ||
(headOffset + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize) ||
(headOffset + DATA_QUEUE_ENTRY_HEADER_SIZE > UINT32_MAX - headSize) ||
(headOffset + headSize + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize)) {
// No room for the header or the data, wrap to the beginning of the queue.
// Note: wrapping even with the UINT32_MAX checks, as we have to support
// queueSize of UINT32_MAX
entry = dataQueue->queue;
} else {
entry = head;
}
}
return entry;
}
void IOSharedDataQueue::free()
{
if (dataQueue) {
IOFreeAligned(dataQueue, round_page(getQueueSize() + DATA_QUEUE_MEMORY_HEADER_SIZE + DATA_QUEUE_MEMORY_APPENDIX_SIZE));
dataQueue = NULL;
if (notifyMsg) {
IOFree(notifyMsg, sizeof(mach_msg_header_t));
notifyMsg = NULL;
}
}
if (_reserved) {
IOFree (_reserved, sizeof(struct ExpansionData));
_reserved = NULL;
}
super::free();
}
static void
handleExecuteCommandAlarm (const AsyncAlarmResult *result) {
Queue *queue = getCommandQueue(0);
if (queue) {
int command = dequeueCommand(queue);
if (command != EOF) {
const CommandHandlerLevel *chl = commandHandlerStack;
while (chl) {
if (chl->handleCommand(command, chl->handlerData)) break;
chl = chl->previousLevel;
}
}
if (getQueueSize(queue) > 0) setExecuteCommandAlarm(result->data);
}
}
inline void BFSTaskRMAFetch::processGlobalChild(Vertex currVertex, Vertex child)
{
Vertex childLocal = graph->vertexToLocal(child);
const int childRank = graph->vertexRank(child);
Vertex parentOfChild;
//printf("%d: Getting parent of child\n", rank);
requestSynch(true, BFS_SYNCH_TAG); //fence is needed now
pWin->fenceOpen(MODE_NOPUT);
pWin->get(&parentOfChild, 1, childRank, childLocal);
pWin->fenceClose(0);
//printf("%d: Parent of child is %ld\n", rank, parentOfChild, numLocalVertex);
assert(0 <= parentOfChild && parentOfChild <= graph->numGlobalVertex);
bool isInnerFenceNeeded = (parentOfChild == graph->numGlobalVertex);
requestSynch(isInnerFenceNeeded, BFS_SYNCH_TAG); // call for inner fence if it is needed
if (isInnerFenceNeeded) {
//printf("%d: Putting child to the parent\n", rank);
pWin->fenceOpen(0);
pWin->put(&currVertex, 1, childRank, childLocal);
pWin->fenceClose(MODE_NOSTORE);
//Updating queue
Vertex queueLastIndex;
//printf("%d: Getting last queue index\n", rank);
nextQWin->fenceOpen(MODE_NOPUT);
nextQWin->get(&queueLastIndex, 1, childRank, 0); // get queue[0]
nextQWin->fenceClose(0);
//printf("%d: Last queue index is %ld\n", rank, queueLastIndex);
assert(0 <= queueLastIndex && queueLastIndex <= getQueueSize());
//printf("%d: Updating queue\n", rank);
nextQWin->fenceOpen(0);
nextQWin->put(&childLocal, 1, childRank, ++queueLastIndex);
nextQWin->put(&queueLastIndex, 1, childRank, 0);
nextQWin->fenceClose(MODE_NOSTORE);
}
}
void WECpiFeederThread::feedData2Cpi()
{
const boost::posix_time::seconds TIME_OUT(2);
while(isContinue())
{
mutex::scoped_lock aLock(fMsgQMutex);
if(fMsgQueue.empty())
{
boost::system_time const abs_time = boost::get_system_time()+ TIME_OUT;
bool aTimedOut = fFeederCond.timed_wait(aLock, abs_time);
if(!isContinue()) { aLock.unlock(); break; }
// to handle spurious wake ups and timeout wake ups
if((fMsgQueue.empty())||(!aTimedOut)) { aLock.unlock(); continue; }
}
messageqcpp::SBS aSbs = fMsgQueue.front();
fMsgQueue.pop();
aLock.unlock();
try
{
fOwner.pushData2Cpimport((*aSbs));
//cout << "Finished PUSHING data " << endl;
}
catch(runtime_error& e)
{
//cout << "Caught exception : " << e.what() << endl;
//break;
}
aSbs.reset(); //forcefully clearing it
// We start sending data request from here ONLY
if(getQueueSize() == WEDataLoader::MAX_QSIZE) fOwner.sendDataRequest();
}
cout << "CpiFeedThread Stopped!! " << endl;
fStopped = true;
}
int
enqueueCommand (int command) {
if (command != EOF) {
Queue *queue = getCommandQueue(1);
if (queue) {
CommandQueueItem *item = malloc(sizeof(CommandQueueItem));
if (item) {
item->command = command;
if (enqueueItem(queue, item)) {
if (getQueueSize(queue) == 1) setExecuteCommandAlarm(NULL);
return 1;
}
free(item);
}
}
}
return 0;
}
void Teller::jockey() {
vector<Teller*>::iterator i;
Teller* jockeyFrom = NULL;
int ni = getQueueSize();
int nj = 0;
unsigned int min_distance = 0xFFFFFFFF;
unsigned int distance;
// caculate the distance between the customer and jocky, recording if there is a jocky avaliabe
for(i=tellerList.begin(); i != tellerList.end(); ++i) {
nj = (*i)->getQueueSize();
distance = abs((*i)->getId() - getId());
if((*i) != this && nj > ni + 1 && distance < min_distance) {
jockeyFrom = (*i);
min_distance = distance;
}
}
// if there is a jockey, then remove that jocky and add it to my own queue
if(jockeyFrom != NULL) {
getBoundQueue("MY_QUEUE")->addLast(jockeyFrom->removeBack());
}
}
//==============================================================================
void AudioFileConverter::run()
{
while ( getQueueSize() > 0 )
{
{ // lock jobQueue before retrieving a task
const ScopedLock lock (queueLock);
task = jobQueue[0];
}
/* try opening the file */
File inputDataFile( task->getFileName() );
String inputFileName( inputDataFile.getFullPathName() );
if ( !inputDataFile.existsAsFile() || (inputDataFile.getSize() == 0) )
{
dbgOut(L"** AudioFileConverter ** Invalid or corrupted temporary file:\t" + inputFileName);
removeFromQueue();
continue;
}
/* try creating the input stream */
FileInputStream* fileInputStream = inputDataFile.createInputStream();
if (fileInputStream == NULL)
{
dbgOut(L"** AudioFileConverter ** Unable to create input stream for file:\t" + inputFileName);
removeFromQueue();
continue;
}
dbgOut(L"");
dbgOut(L" *** AudioFileConverter ***");
dbgOut(L"** AudioFileConverter ** Converting file:\t" + inputFileName
+ L" (" + String( inputDataFile.getSize() ) + L" b)");
int processorOutputs = task->getChannelNumber();
const int bytesPerSample = processorOutputs * sizeof(float);
int bufferSize = task->getBufferSize();
double samplingRate = task->getSamplingRate();
int bitDepth = task->getBitDepth();
String audioFormatName = task->getFormat();
AudioSampleBuffer tempBuffer(1, bufferSize);
// declare classes needed to save the format
OwnedArray<AudioFormat> someAudioFormats;
OwnedArray<AudioFormatWriter> audioFormatWriters;
OwnedArray<File> audioFiles;
Array<FileOutputStream*> outStreams;
String audioFileName;
AudioFormatWriter* tmpWriter;
FileOutputStream* tmpStream;
File* tmpAudioFile;
String outputDir = inputDataFile.getParentDirectory().getFullPathName();
for (int i=0; i < processorOutputs ; i++)
{
// Delete temporary files
File tmpDataFile(outputDir + File::separatorString + L"channel" + String::formatted("%.2d", i ) + ".dat");
if ( tmpDataFile != File::nonexistent)
{
dbgOut( L"** AudioFileConverter ** \tDeleting temporary file:\t" + tmpDataFile.getFullPathName() );
tmpDataFile.deleteFile();
}
else
{
dbgOut( "** AudioFileConverter ** Unable to delete temporary file:\t\t" + tmpDataFile.getFullPathName() );
}
// Define the format (wav is default)
if (audioFormatName == "wav")
someAudioFormats.add( new WavAudioFormat() );
else if (audioFormatName == "aiff")
someAudioFormats.add( new AiffAudioFormat() );
else if (audioFormatName == "flac")
someAudioFormats.add( new FlacAudioFormat() );
// else if (audioFormatName == "ogg")
// someAudioFormats.add( new OggVorbisAudioFormat() );
else
someAudioFormats.add( new WavAudioFormat() );
audioFileName = outputDir + File::separatorString + "channel" + String::formatted("%.2d",i) + someAudioFormats[i]->getFileExtensions()[0];
tmpAudioFile = new File (audioFileName);
if (*tmpAudioFile == File::nonexistent)
{
dbgOut( L"** AudioFileConverter ** Unable to create file:\t" + audioFileName );
audioFormatWriters.clear(true);
someAudioFormats.clear(true);
audioFiles.clear(true);
outStreams.clear();
delete fileInputStream;
removeFromQueue();
continue;
}
audioFiles.add( tmpAudioFile );
// Delete existing files
if (audioFiles[i]->existsAsFile())
{
dbgOut( "** AudioFileConverter ** \tDeleting existing audio file:\t\t" + audioFileName );
if (!audioFiles[i]->deleteFile())
{
dbgOut( L"** AudioFileConverter ** Unable to delete existing file:\t" + audioFileName );
audioFormatWriters.clear(true);
someAudioFormats.clear(true);
audioFiles.clear(true);
outStreams.clear();
delete fileInputStream;
removeFromQueue();
continue;
}
}
dbgOut( "** AudioFileConverter ** \tSaving audio file:\t\t" + audioFileName );
/* Create output stream for this file */
tmpStream = audioFiles[i]->createOutputStream();
if (tmpStream == NULL)
{
dbgOut( L"** AudioFileConverter ** Unable to create output stream for file:\t" + audioFileName );
delete tmpAudioFile;
audioFormatWriters.clear(true);
someAudioFormats.clear(true);
audioFiles.clear(true);
outStreams.clear();
delete fileInputStream;
removeFromQueue();
continue;
}
outStreams.add( tmpStream );
/* Create Audio Format Writer */
tmpWriter = someAudioFormats[i]->createWriterFor( outStreams[i], // streamToWriteTo,
samplingRate, // sampleRateToUse,
1, // numberOfChannels,
someAudioFormats[i]->getPossibleBitDepths().getLast(), // bitsPerSample - Get the maximum possible bit depth for this format
NULL, // metadataValues,
0 );
if (tmpWriter == NULL)
{
dbgOut( L"** AudioFileConverter ** Unable to create audio format writer for:\t" + audioFileName );
delete tmpAudioFile;
audioFormatWriters.clear(true);
someAudioFormats.clear(true);
audioFiles.clear(true);
outStreams.clear();
delete fileInputStream;
removeFromQueue();
continue;
}
audioFormatWriters.add( tmpWriter );
}
// Write data to wav file
int dataBlockSize = processorOutputs * bufferSize * bitDepth/8 ;
MemoryBlock* buffer = new MemoryBlock( dataBlockSize, true);
int64 bytesSaved = inputDataFile.getSize();
while ( !fileInputStream->isExhausted() && (fileInputStream->getPosition() < bytesSaved) )
{
float* x = (float *) buffer->getData() ;
int bytesRead = fileInputStream->read( (void *)x, dataBlockSize );
int numSamples = (int)( bytesRead / bytesPerSample );
for (int ch=0; ch < processorOutputs; ch++)
{
// const int numBytes = (int) (bytesRead/processorOutputs);
tempBuffer.copyFrom( 0, // const int destChannel,
0, // const int destStartSample,
x+ch*numSamples, // const float * source,
numSamples // int numSamples
);
audioFormatWriters[ch]->write( (const int**)(tempBuffer.getArrayOfChannels()), //AudioFormatWriter * writer,
numSamples //const int numSamples
);
}
}
// clean up
delete buffer;
// this should delete 'owned' objects
audioFormatWriters.clear(true);
someAudioFormats.clear(true);
audioFiles.clear(true);
// clear the outStreams without deleting objects (already deleted)
outStreams.clear();
// Delete and close the stream
delete fileInputStream;
// Delete the data.dat file
dbgOut( L"** AudioFileConverter ** \tDeleting temporary file:\t" + inputFileName );
inputDataFile.deleteFile();
// Delete the task
removeFromQueue();
dbgOut( "** AudioFileConverter ** Files saved." );
}
dbgOut( "** AudioFileConverter ** Thread terminates." );
}
Boolean IOSharedDataQueue::enqueue(void * data, UInt32 dataSize)
{
UInt32 head;
UInt32 tail;
UInt32 newTail;
const UInt32 entrySize = dataSize + DATA_QUEUE_ENTRY_HEADER_SIZE;
IODataQueueEntry * entry;
// Force a single read of head and tail
// See rdar://problem/40780584 for an explanation of relaxed/acquire barriers
tail = __c11_atomic_load((_Atomic UInt32 *)&dataQueue->tail, __ATOMIC_RELAXED);
head = __c11_atomic_load((_Atomic UInt32 *)&dataQueue->head, __ATOMIC_ACQUIRE);
// Check for overflow of entrySize
if (dataSize > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) {
return false;
}
// Check for underflow of (getQueueSize() - tail)
if (getQueueSize() < tail || getQueueSize() < head) {
return false;
}
if ( tail >= head )
{
// Is there enough room at the end for the entry?
if ((entrySize <= UINT32_MAX - tail) &&
((tail + entrySize) <= getQueueSize()) )
{
entry = (IODataQueueEntry *)((UInt8 *)dataQueue->queue + tail);
entry->size = dataSize;
memcpy(&entry->data, data, dataSize);
// The tail can be out of bound when the size of the new entry
// exactly matches the available space at the end of the queue.
// The tail can range from 0 to dataQueue->queueSize inclusive.
newTail = tail + entrySize;
}
else if ( head > entrySize ) // Is there enough room at the beginning?
{
// Wrap around to the beginning, but do not allow the tail to catch
// up to the head.
dataQueue->queue->size = dataSize;
// We need to make sure that there is enough room to set the size before
// doing this. The user client checks for this and will look for the size
// at the beginning if there isn't room for it at the end.
if ( ( getQueueSize() - tail ) >= DATA_QUEUE_ENTRY_HEADER_SIZE )
{
((IODataQueueEntry *)((UInt8 *)dataQueue->queue + tail))->size = dataSize;
}
memcpy(&dataQueue->queue->data, data, dataSize);
newTail = entrySize;
}
else
{
return false; // queue is full
}
}
else
{
// Do not allow the tail to catch up to the head when the queue is full.
// That's why the comparison uses a '>' rather than '>='.
if ( (head - tail) > entrySize )
{
entry = (IODataQueueEntry *)((UInt8 *)dataQueue->queue + tail);
entry->size = dataSize;
memcpy(&entry->data, data, dataSize);
newTail = tail + entrySize;
}
else
{
return false; // queue is full
}
}
// Publish the data we just enqueued
__c11_atomic_store((_Atomic UInt32 *)&dataQueue->tail, newTail, __ATOMIC_RELEASE);
if (tail != head) {
//
// The memory barrier below paris with the one in ::dequeue
// so that either our store to the tail cannot be missed by
// the next dequeue attempt, or we will observe the dequeuer
// making the queue empty.
//
// Of course, if we already think the queue is empty,
// there's no point paying this extra cost.
//
__c11_atomic_thread_fence(__ATOMIC_SEQ_CST);
head = __c11_atomic_load((_Atomic UInt32 *)&dataQueue->head, __ATOMIC_RELAXED);
}
if (tail == head) {
// Send notification (via mach message) that data is now available.
sendDataAvailableNotification();
}
return true;
}
Boolean IOSharedDataQueue::dequeue(void *data, UInt32 *dataSize)
{
Boolean retVal = TRUE;
IODataQueueEntry * entry = 0;
UInt32 entrySize = 0;
UInt32 headOffset = 0;
UInt32 tailOffset = 0;
UInt32 newHeadOffset = 0;
if (!dataQueue || (data && !dataSize)) {
return false;
}
// Read head and tail with acquire barrier
// See rdar://problem/40780584 for an explanation of relaxed/acquire barriers
headOffset = __c11_atomic_load((_Atomic UInt32 *)&dataQueue->head, __ATOMIC_RELAXED);
tailOffset = __c11_atomic_load((_Atomic UInt32 *)&dataQueue->tail, __ATOMIC_ACQUIRE);
if (headOffset != tailOffset) {
IODataQueueEntry * head = 0;
UInt32 headSize = 0;
UInt32 queueSize = getQueueSize();
if (headOffset > queueSize) {
return false;
}
head = (IODataQueueEntry *)((char *)dataQueue->queue + headOffset);
headSize = head->size;
// we wrapped around to beginning, so read from there
// either there was not even room for the header
if ((headOffset > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) ||
(headOffset + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize) ||
// or there was room for the header, but not for the data
(headOffset + DATA_QUEUE_ENTRY_HEADER_SIZE > UINT32_MAX - headSize) ||
(headOffset + headSize + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize)) {
// Note: we have to wrap to the beginning even with the UINT32_MAX checks
// because we have to support a queueSize of UINT32_MAX.
entry = dataQueue->queue;
entrySize = entry->size;
if ((entrySize > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) ||
(entrySize + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize)) {
return false;
}
newHeadOffset = entrySize + DATA_QUEUE_ENTRY_HEADER_SIZE;
// else it is at the end
} else {
entry = head;
entrySize = entry->size;
if ((entrySize > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) ||
(entrySize + DATA_QUEUE_ENTRY_HEADER_SIZE > UINT32_MAX - headOffset) ||
(entrySize + DATA_QUEUE_ENTRY_HEADER_SIZE + headOffset > queueSize)) {
return false;
}
newHeadOffset = headOffset + entrySize + DATA_QUEUE_ENTRY_HEADER_SIZE;
}
} else {
// empty queue
return false;
}
if (data) {
if (entrySize > *dataSize) {
// not enough space
return false;
}
memcpy(data, &(entry->data), entrySize);
*dataSize = entrySize;
}
__c11_atomic_store((_Atomic UInt32 *)&dataQueue->head, newHeadOffset, __ATOMIC_RELEASE);
if (newHeadOffset == tailOffset) {
//
// If we are making the queue empty, then we need to make sure
// that either the enqueuer notices, or we notice the enqueue
// that raced with our making of the queue empty.
//
__c11_atomic_thread_fence(__ATOMIC_SEQ_CST);
}
return retVal;
}
//--------------------------------------------------------------
void ofxThreadedVideo::finish(){
while(getQueueSize() > 0){
update();
}
update();
}
int
asyncExecuteIoCallback (AsyncIoData *iod, long int timeout) {
if (iod) {
Queue *functions = iod->functionQueue;
unsigned int functionCount = functions? getQueueSize(functions): 0;
prepareMonitors();
if (functionCount) {
MonitorEntry monitorArray[functionCount];
MonitorGroup monitors = {
.array = monitorArray,
.count = 0
};
int executed = 0;
Element *functionElement = processQueue(functions, addFunctionMonitor, &monitors);
if (!functionElement) {
if (!monitors.count) {
approximateDelay(timeout);
} else if (awaitMonitors(&monitors, timeout)) {
functionElement = processQueue(functions, testFunctionMonitor, NULL);
}
}
if (functionElement) {
FunctionEntry *function = getElementItem(functionElement);
Element *operationElement = getActiveOperationElement(function);
OperationEntry *operation = getElementItem(operationElement);
if (!operation->finished) finishOperation(operation);
operation->active = 1;
if (!function->methods->invokeCallback(operation)) operation->cancel = 1;
operation->active = 0;
executed = 1;
if (operation->cancel) {
deleteElement(operationElement);
} else {
operation->error = 0;
}
if ((operationElement = getActiveOperationElement(function))) {
operation = getElementItem(operationElement);
if (!operation->finished) startOperation(operation);
requeueElement(functionElement);
} else {
deleteElement(functionElement);
}
}
return executed;
}
}
approximateDelay(timeout);
return 0;
}
static void
deallocateOperationEntry (void *item, void *data) {
OperationEntry *operation = item;
if (operation->extension) free(operation->extension);
free(operation);
}
bool IOSharedEventQueue::EnqueueTracker(DataArgs * data)
{
uint32_t singleTrackerLen = sizeof(DataArgs);
const UInt32 head = dataQueue->head;
const UInt32 tail = dataQueue->tail;
LOG(LOG_DEBUG, "head=%d", dataQueue->head);
LOG(LOG_DEBUG, "tail=%d", dataQueue->tail);
const UInt32 entrySize = singleTrackerLen+DATA_QUEUE_ENTRY_HEADER_SIZE;
IODataQueueEntry *entry;
if(singleTrackerLen>UINT32_MAX-DATA_QUEUE_ENTRY_HEADER_SIZE)
{
return false;
}
LOG(LOG_DEBUG, "this->getQueueSize()=%d", this->getQueueSize());
if(this->getQueueSize()<tail)
{
return false;
}
if(tail>=head)
{
if(entrySize<=UINT32_MAX-DATA_QUEUE_ENTRY_HEADER_SIZE &&
tail+entrySize<=this->getQueueSize())
{
entry = (IODataQueueEntry*)((uint8_t*)dataQueue->queue+dataQueue->tail);
entry->size=singleTrackerLen;
memcpy(entry->data, data, singleTrackerLen);
OSAddAtomic(entrySize, (SInt32*)&(dataQueue->tail));
}
else if(head>singleTrackerLen)
{
dataQueue->queue->size = singleTrackerLen;
if ( ( getQueueSize() - tail ) >= DATA_QUEUE_ENTRY_HEADER_SIZE )
{
((IODataQueueEntry *)((UInt8 *)dataQueue->queue + tail))->size = entrySize;
}
memcpy(&dataQueue->queue->data, data, singleTrackerLen);
OSCompareAndSwap(dataQueue->tail, entrySize, &dataQueue->tail);
}
else
{
return false;
}
}
else
{
if ( (head - tail) > entrySize )
{
entry = (IODataQueueEntry *)((UInt8 *)dataQueue->queue + tail);
entry->size = singleTrackerLen;
memcpy(&entry->data, data, singleTrackerLen);
OSAddAtomic(entrySize, (SInt32 *)&dataQueue->tail);
}
else
{
return false; // queue is full
}
}
if(head==tail) return true;
//send notification to port if any data is added to queue.
//if ( (this->_status&kSharedEventQueueNotifyWhenAddData) || ( head == tail ) || ( dataQueue->head == tail ))
{
sendDataAvailableNotification();
}
return true;
}
StereoUndistort::StereoUndistort(const ros::NodeHandle& nh,
const ros::NodeHandle& nh_private)
: nh_(nh),
nh_private_(nh_private),
it_(nh_),
queue_size_(getQueueSize()),
first_image_sub_(it_, "raw/first/image", queue_size_),
second_image_sub_(it_, "raw/second/image", queue_size_),
first_undistorter_ptr_(nullptr),
second_undistorter_ptr_(nullptr),
frame_counter_(0) {
// set parameters from ros
nh_private_.param("input_camera_info_from_ros_params",
input_camera_info_from_ros_params_,
kDefaultInputCameraInfoFromROSParams);
nh_private_.param("rename_radtan_plumb_bob", rename_radtan_plumb_bob_,
kDefaultRenameRadtanPlumbBob);
bool invert_T;
nh_private_.param("invert_T", invert_T, kDefaultInvertT);
nh_private_.param("queue_size", queue_size_, kQueueSize);
if (queue_size_ < 1) {
ROS_ERROR("Queue size must be >= 1, setting to 1");
queue_size_ = 1;
}
double scale;
nh_private_.param("scale", scale, kDefaultScale);
stereo_camera_parameters_ptr_ =
std::make_shared<StereoCameraParameters>(scale);
nh_private_.param("process_every_nth_frame", process_every_nth_frame_,
kDefaultProcessEveryNthFrame);
nh_private_.param("output_image_type", output_image_type_,
kDefaultOutputImageType);
// check output type string is correctly formatted
if (!output_image_type_.empty()) {
try {
cv_bridge::getCvType(output_image_type_);
} catch (const cv_bridge::Exception& e) {
ROS_ERROR_STREAM(
"cv_bridge error while setting output_image_type, output will match "
"input type. "
<< e.what());
output_image_type_ = "";
}
}
nh_private_.param("publish_tf", publish_tf_, kDefaultPublishTF);
nh_private_.param("first_output_frame", first_output_frame_,
kDefaultFirstOutputFrame);
if (first_output_frame_.empty()) {
ROS_ERROR("First output frame cannot be blank, setting to default");
first_output_frame_ = kDefaultFirstOutputFrame;
}
nh_private_.param("second_output_frame", second_output_frame_,
kDefaultSecondOutputFrame);
if (second_output_frame_.empty()) {
ROS_ERROR("Second output frame cannot be blank, setting to default");
second_output_frame_ = kDefaultSecondOutputFrame;
}
nh_private_.param("rename_input_frame", rename_input_frame_,
kDefaultRenameInputFrame);
nh_private_.param("first_input_frame", first_input_frame_,
kDefaultFirstInputFrame);
if (first_input_frame_.empty()) {
ROS_ERROR("First input frame cannot be blank, setting to default");
first_input_frame_ = kDefaultFirstInputFrame;
}
nh_private_.param("second_input_frame", second_input_frame_,
kDefaultSecondInputFrame);
if (second_input_frame_.empty()) {
ROS_ERROR("Second input frame cannot be blank, setting to default");
second_input_frame_ = kDefaultSecondInputFrame;
}
// setup publishers
first_camera_info_output_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(
"rect/first/camera_info", queue_size_);
second_camera_info_output_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(
"rect/second/camera_info", queue_size_);
first_image_pub_ = it_.advertise("rect/first/image", queue_size_);
second_image_pub_ = it_.advertise("rect/second/image", queue_size_);
// setup subscribers (must be done last as it appears image filters allow a
// subscriber to be called as soon as it is created, even if this constructor
// is not finished)
if (input_camera_info_from_ros_params_) {
std::string first_camera_namespace, second_camera_namespace;
nh_private_.param("first_camera_namespace", first_camera_namespace,
kDefaultFirstCameraNamespace);
nh_private_.param("second_camera_namespace", second_camera_namespace,
kDefaultSecondCameraNamespace);
if (!stereo_camera_parameters_ptr_->setInputCameraParameters(
nh_private_, first_camera_namespace, CameraSide::FIRST, invert_T) ||
!stereo_camera_parameters_ptr_->setInputCameraParameters(
nh_private_, second_camera_namespace, CameraSide::SECOND,
invert_T)) {
ROS_FATAL("Loading of input camera parameters failed, exiting");
ros::shutdown();
exit(EXIT_FAILURE);
}
first_camera_info_input_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(
"raw/first/camera_info", queue_size_);
second_camera_info_input_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(
"raw/second/camera_info", queue_size_);
image_sync_ptr_ =
std::make_shared<message_filters::Synchronizer<ImageSyncPolicy>>(
ImageSyncPolicy(queue_size_), first_image_sub_, second_image_sub_);
image_sync_ptr_->registerCallback(
boost::bind(&StereoUndistort::imagesCallback, this, _1, _2));
} else {
first_camera_info_sub_ptr_ =
std::make_shared<message_filters::Subscriber<sensor_msgs::CameraInfo>>(
nh_, "raw/first/camera_info", queue_size_);
second_camera_info_sub_ptr_ =
std::make_shared<message_filters::Subscriber<sensor_msgs::CameraInfo>>(
nh_, "raw/second/camera_info", queue_size_);
camera_sync_ptr_ =
std::make_shared<message_filters::Synchronizer<CameraSyncPolicy>>(
CameraSyncPolicy(queue_size_), first_image_sub_, second_image_sub_,
*first_camera_info_sub_ptr_, *second_camera_info_sub_ptr_);
camera_sync_ptr_->registerCallback(
boost::bind(&StereoUndistort::camerasCallback, this, _1, _2, _3, _4));
}
}