148 lines
3.7 KiB
C++
Executable File
148 lines
3.7 KiB
C++
Executable File
|
|
|
|
|
|
|
|
#include "wait_set_handler.h"
|
|
#include "generic_logger.h"
|
|
#include "generic_base.h"
|
|
|
|
namespace DDSGeneric
|
|
{
|
|
|
|
WaitSetHandler::WaitSetHandler()
|
|
: m_logger(DDS::getLogger("WaitSetHandler", DDS::DataType::FUNCTION_TOPIC_SERVER))
|
|
{
|
|
m_waitSet.attach_condition(m_abortCondition);
|
|
m_task = std::async(std::launch::async, [this]() { this->waitSetTask();});
|
|
}
|
|
|
|
WaitSetHandler::~WaitSetHandler()
|
|
{
|
|
m_abortCondition.trigger_value(true);
|
|
m_task.wait();
|
|
}
|
|
|
|
bool WaitSetHandler::attachWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb)
|
|
{
|
|
DEBUG_LOG(m_logger) << "AttachWaitSet with callback " << cb;
|
|
|
|
WaitSetData wsd(cond, cb);
|
|
|
|
std::lock_guard<std::mutex> l(m_accessMutex);
|
|
|
|
auto it = m_waitSetData.find(wsd.condition->native_condition());
|
|
if (it != m_waitSetData.end())
|
|
{
|
|
WARNING_LOG(m_logger) << "Waitset " << cb << " already registered";
|
|
return false;
|
|
}
|
|
|
|
m_waitSet.attach_condition(wsd.condition);
|
|
wsd.enabled = true;
|
|
wsd.needWaitSetAttach = false;
|
|
|
|
m_waitSetData.insert(std::make_pair(wsd.condition->native_condition(), wsd));
|
|
return true;
|
|
}
|
|
|
|
bool WaitSetHandler::detachWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb)
|
|
{
|
|
DEBUG_LOG(m_logger) << "DetachWaitSet with callback " << cb;
|
|
|
|
WaitSetData wsd(cond, cb);
|
|
|
|
std::lock_guard<std::mutex> l(m_accessMutex);
|
|
|
|
auto it = m_waitSetData.find(wsd.condition->native_condition());
|
|
if (it == m_waitSetData.end())
|
|
{
|
|
WARNING_LOG(m_logger) << "Waitset for " << cb << " not registered";
|
|
return false;
|
|
}
|
|
|
|
m_waitSet.detach_condition(wsd.condition);
|
|
m_waitSetData.erase(it);
|
|
return true;
|
|
}
|
|
|
|
bool WaitSetHandler::enableWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb)
|
|
{
|
|
DEBUG_LOG(m_logger) << "EnableWaitSet with callback " << cb;
|
|
|
|
WaitSetData wsd(cond, cb);
|
|
|
|
std::lock_guard<std::mutex> l(m_accessMutex);
|
|
auto it = m_waitSetData.find(wsd.condition->native_condition());
|
|
if (it == m_waitSetData.end())
|
|
{
|
|
ERROR_LOG(m_logger) << "Could not find WaitSetData - cannot enable waitSet for " << cb;
|
|
return false;
|
|
}
|
|
|
|
if (it->second.needWaitSetAttach)
|
|
m_waitSet.attach_condition(wsd.condition);
|
|
it->second.enabled = true;
|
|
it->second.needWaitSetAttach = false;
|
|
return true;
|
|
}
|
|
|
|
bool WaitSetHandler::disableWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb)
|
|
{
|
|
DEBUG_LOG(m_logger) << "DisableWaitSet with callback " << cb;
|
|
|
|
WaitSetData wsd(cond, cb);
|
|
|
|
std::lock_guard<std::mutex> l(m_accessMutex);
|
|
auto it = m_waitSetData.find(wsd.condition->native_condition());
|
|
if (it == m_waitSetData.end())
|
|
{
|
|
ERROR_LOG(m_logger) << "Could not find WaitSetData - cannot disable waitSet for " << cb;
|
|
return false;
|
|
}
|
|
|
|
m_waitSet.detach_condition(wsd.condition);
|
|
it->second.enabled = false;
|
|
it->second.needWaitSetAttach = true;
|
|
return true;
|
|
}
|
|
|
|
int WaitSetHandler::attachedConditionCount() const
|
|
{
|
|
return m_waitSet.conditions().size();
|
|
}
|
|
|
|
|
|
void WaitSetHandler::waitSetTask()
|
|
{
|
|
while (true)
|
|
{
|
|
dds::core::cond::WaitSet::ConditionSeq activeConditionsSeq = m_waitSet.wait();
|
|
for (const dds::core::cond::Condition &cond : activeConditionsSeq)
|
|
{
|
|
if (cond == m_abortCondition)
|
|
{
|
|
m_waitSet->detach_all();
|
|
return;
|
|
}
|
|
|
|
std::lock_guard<std::mutex> l(m_accessMutex);
|
|
const DDS_Condition *ddsCond = cond->native_condition();
|
|
auto it = m_waitSetData.lower_bound(ddsCond);
|
|
while (it != m_waitSetData.end())
|
|
{
|
|
WaitSetData &wsd = it->second;
|
|
if (wsd.enabled)
|
|
{
|
|
DEBUG_LOG(m_logger) << "Condition triggered for " << wsd.callback;
|
|
wsd.callback->dataAvailable();
|
|
}
|
|
++it;
|
|
if (it == m_waitSetData.end() || it->first != ddsCond)
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|