Initial commit

Lts/DDS/Generic/wait_set_handler.cpp

@@ -0,0 +1,152 @@
+/**
+  * @RDE_HEADER@
+  * Handler which notifes the FTS-instances about new data
+  * $Id$
+  * $HeadURL$
+  * $Author$
+  */
+
+
+#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);
+  // already there -> do not add a second time
+  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);
+  // not there -> no remove possible
+  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;
+      }
+    }
+  }
+}
+
+} // namespace DDSGeneric