// ******************************************
// O(N) traversal
int Traverse(unsigned long *random_seed, param_t *params)
{
rbnode_t *new_node, *node;
long key = -1;
#ifdef DEBUG
long values[1000];
index = 0;
//printf("TRAVERSAL ******************************************\n");
#endif
#ifdef NO_GRACE_PERIOD
read_lock(My_Tree->lock);
#else
rw_lock(My_Tree->lock);
#endif
new_node = rb_first_n(My_Tree);
assert(new_node->key == -1);
while (new_node != NULL)
{
node = new_node;
key = node->key;
#ifdef DEBUG
values[index++] = key;
#endif
new_node = rb_next(node);
#ifdef DEBUG
if (new_node != NULL && node->key >= new_node->key)
{
printf("******************************************\n"
"TRAVERSEAL ERROR key: %ld new: %ld\n"
"******************************************\n",
node->key, new_node->key);
while (--index >= 0)
{
printf("%3d: %ld\n", index, values[index]);
}
#ifdef NO_GRACE_PERIOD
read_unlock(My_Tree->lock);
#else
rw_unlock(My_Tree->lock);
#endif
write_lock(My_Tree->lock);
rb_output(My_Tree);
exit(-1);
return 0;
}
#endif
}
#ifdef NO_GRACE_PERIOD
read_unlock(My_Tree->lock);
#else
rw_unlock(My_Tree->lock);
#endif
assert(key == params->scale + 1);
return 0;
}
CallbackQueue::CallOneResult CallbackQueue::callOneCB(TLS* tls)
{
// Check for a recursive call. If recursive, increment the current iterator. Otherwise
// set the iterator it the beginning of the thread-local callbacks
if (tls->calling_in_this_thread == 0xffffffffffffffffULL)
{
tls->cb_it = tls->callbacks.begin();
}
if (tls->cb_it == tls->callbacks.end())
{
return Empty;
}
ROS_ASSERT(!tls->callbacks.empty());
ROS_ASSERT(tls->cb_it != tls->callbacks.end());
CallbackInfo info = *tls->cb_it;
CallbackInterfacePtr& cb = info.callback;
IDInfoPtr id_info = getIDInfo(info.removal_id);
if (id_info)
{
boost::shared_lock<boost::shared_mutex> rw_lock(id_info->calling_rw_mutex);
uint64_t last_calling = tls->calling_in_this_thread;
tls->calling_in_this_thread = id_info->id;
CallbackInterface::CallResult result = CallbackInterface::Invalid;
if (info.marked_for_removal)
{
tls->cb_it = tls->callbacks.erase(tls->cb_it);
}
else
{
tls->cb_it = tls->callbacks.erase(tls->cb_it);
result = cb->call();
}
tls->calling_in_this_thread = last_calling;
// Push TryAgain callbacks to the back of the shared queue
if (result == CallbackInterface::TryAgain && !info.marked_for_removal)
{
boost::mutex::scoped_lock lock(mutex_);
callbacks_.push_back(info);
return TryAgain;
}
return Called;
}
else
{
tls->cb_it = tls->callbacks.erase(tls->cb_it);
}
return Called;
}
void CallbackQueue::removeByID(uint64_t removal_id)
{
setupTLS();
{
IDInfoPtr id_info;
{
boost::mutex::scoped_lock lock(id_info_mutex_);
M_IDInfo::iterator it = id_info_.find(removal_id);
if (it != id_info_.end())
{
id_info = it->second;
}
else
{
return;
}
}
// If we're being called from within a callback from our queue, we must unlock the shared lock we already own
// here so that we can take a unique lock. We'll re-lock it later.
if (tls_->calling_in_this_thread == id_info->id)
{
id_info->calling_rw_mutex.unlock_shared();
}
{
boost::unique_lock<boost::shared_mutex> rw_lock(id_info->calling_rw_mutex);
boost::mutex::scoped_lock lock(mutex_);
D_CallbackInfo::iterator it = callbacks_.begin();
for (; it != callbacks_.end();)
{
CallbackInfo& info = *it;
if (info.removal_id == removal_id)
{
it = callbacks_.erase(it);
}
else
{
++it;
}
}
}
if (tls_->calling_in_this_thread == id_info->id)
{
id_info->calling_rw_mutex.lock_shared();
}
}
// If we're being called from within a callback, we need to remove the callbacks that match the id that have already been
// popped off the queue
{
D_CallbackInfo::iterator it = tls_->callbacks.begin();
D_CallbackInfo::iterator end = tls_->callbacks.end();
for (; it != end; ++it)
{
CallbackInfo& info = *it;
if (info.removal_id == removal_id)
{
info.marked_for_removal = true;
}
}
}
{
boost::mutex::scoped_lock lock(id_info_mutex_);
id_info_.erase(removal_id);
}
}
