commit 33aa05c5588c4f3d049704965ec0b2fe799ee2d0
Author: Svenja <skenneweg@techfak.uni-bielefeld.de>
Date:   Thu Jul 30 16:46:45 2020 +0200

    Started with the publisher

diff --git a/inc/urt_core.h b/inc/urt_core.h
index a7e097e..2cc241a 100644
--- a/inc/urt_core.h
+++ b/inc/urt_core.h
@@ -63,17 +63,17 @@ extern "C" {
   urt_node_t* urtCoreGetNodes(void);
   urt_status_t urtCoreAddNode(urt_node_t* node);
 
-#if (URT_CFG_PUBSUB_ENABLED)
+#if (URT_CFG_PUBSUB_ENABLED == true)
   urt_status_t urtCoreAddTopic(urt_topic_t* topic);
   urt_topic_t* urtCoreGetTopic(urt_topicid_t id);
 #endif /* URT_CFG_PUBSUB_ENABLED */
 
-#if (URT_CFG_RPC_ENABLED)
+#if (URT_CFG_RPC_ENABLED == true)
   urt_status_t urtCoreAddService(urt_service_t* service);
   urt_service_t* urtCoreGetService(urt_serviceid_t id);
 #endif /* URT_CFG_RPC_ENABLED */
 
-#if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS)
+#if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)
   urt_osTimerCallback_t urtCoreCallbackDefault(void* params)
 #endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */
 
diff --git a/inc/urt_publisher.h b/inc/urt_publisher.h
index 99be719..932f650 100644
--- a/inc/urt_publisher.h
+++ b/inc/urt_publisher.h
@@ -22,7 +22,6 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #ifndef URTWARE_PUBLISHER_H
 #define URTWARE_PUBLISHER_H
 
-#include <urt_types.h>
 #include <urtware.h>
 
 /******************************************************************************/
@@ -65,7 +64,7 @@ typedef struct urt_publisher
 extern "C" {
 #endif /* defined(__cplusplus) */
 
-  void urtPublisherInit(urt_publisher_t* publisher, urt_topic_t* topic, urt_message_t* message);
+  void urtPublisherInit(urt_publisher_t* publisher, urt_topic_t* topic, urt_message_t* messages);
   urt_status_t urtPublisherPublish(urt_publisher_t* publisher, void* payload, size_t bytes, urt_osTime_t t, urt_delay_t timeout);
 #if defined(__cplusplus)
 }
diff --git a/src/urt_core.c b/src/urt_core.c
index 38d3bb5..a3acacf 100644
--- a/src/urt_core.c
+++ b/src/urt_core.c
@@ -128,7 +128,6 @@ void urtCoreStartNodes(void)
   {
     urtThreadStart(node->thread);
     node = node->next;
-    urtThreadMSleep(100); //TODO: delete
   }
   urtMutexUnlock(&core._lock);
   return;
@@ -208,8 +207,9 @@ urt_status_t urtCoreStopNodes(urt_status_t reason)
     urt_osEventFlags_t flag = URT_EVENTFLAG_TERMINATE;
     urtEventSourceBroadcast(&core._evtSource, flag);
     urtMutexUnlock(&core._lock);
-    if (priorityBoosted)
+    if (priorityBoosted) {
       core._nodes->thread->prio = oldPrio;
+    }
     return URT_STATUS_OK;
   }
   else
@@ -256,7 +256,7 @@ urt_status_t urtCoreAddNode(urt_node_t* node)
  *
  * @return  Returns a pointer to the requested service. Returns NULL if no service matches the given ID.
  */
-# if (URT_CFG_PUBSUB_ENABLED)
+#if (URT_CFG_PUBSUB_ENABLED == true)
 
 /**
  * @brief   Append topic to core's list of topics.
@@ -295,7 +295,7 @@ urt_topic_t* urtCoreGetTopic(urt_topicid_t id)
   else
     return NULL;
 }
-# endif /* URT_CFG_PUBSUB_ENABLED */
+#endif /* URT_CFG_PUBSUB_ENABLED */
 
 
 # if (URT_CFG_RPC_ENABLED)
@@ -327,7 +327,7 @@ urt_status_t urtCoreAddService(urt_service_t* service)
 urt_service_t* urtCoreGetService(urt_serviceid_t id) {return urt_service_t;}
 # endif /* URT_CFG_RPC_ENABLED */
 
-#if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS)
+#if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)
 urt_osTimerCallback_t urtCoreCallbackDefault(void* params)
 {
   urtMutexLock(&core._lock);
diff --git a/src/urt_node.c b/src/urt_node.c
index 5b772f5..bfeebc0 100644
--- a/src/urt_node.c
+++ b/src/urt_node.c
@@ -69,7 +69,7 @@ void _main(void* arg)
     urtCoreSynchronizeNodes(((urt_node_t*)arg));
   }
 
-  while (urtThreadShouldTerminate())
+  while (!urtThreadShouldTerminate())
   {
     urt_osEventMask_t temp = urtEventWait(mask, URT_EVENT_WAIT_ONE, URT_DELAY_INFINITE);
     if (temp == urtCoreGetEventMask())
@@ -91,7 +91,6 @@ void _main(void* arg)
   }
   urtEventUnregister(urtCoreGetEvtSource(), &((urt_node_t*)arg)->listener);
 
-
   //Terminate all children and siblings
   urt_osThread_t* threadToTerminate = urtThreadGetSelf()->children;
   while (threadToTerminate != urtThreadGetSelf() && threadToTerminate != NULL)
@@ -199,10 +198,7 @@ void urtNodeInit(urt_node_t* node, urt_osThread_t* thread, urt_osThreadPrio_t pr
   #if (URT_CFG_PUBSUB_PROFILING || URT_CFG_RPC_PROFILING)
     node->loops = 0;
   #endif /* URT_CFG_PUBSUB_PROFILING || URT_CFG_RPC_PROFILING */
-  urt_osMutex_t* mutexTemp = urtCoreGetMutex();
-  urtMutexLock(mutexTemp);
-    node->next = urtCoreGetNodes();
-    urtCoreAddNode(node);
-  urtMutexUnlock(mutexTemp);
+  node->next = urtCoreGetNodes();
+  urtCoreAddNode(node);
   return;
 }
diff --git a/src/urt_publisher.c b/src/urt_publisher.c
index b128800..6b44295 100644
--- a/src/urt_publisher.c
+++ b/src/urt_publisher.c
@@ -46,7 +46,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 /******************************************************************************/
 
 /**
- * @brief   Initalize the publisher.
+ * @brief   Initialize the publisher.
  *
  * @param[in] publisher  The publisher to initialize. Must not be NULL.
  * @param[in] topic  The topic, this publisher is associated to. Must not be NULL.
@@ -55,14 +55,26 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
  *                     Once a message has been contributed, it cannot be removed later.
  *                     May be NULL (no messages to contribute).
  */
-void urtPublisherInit(urt_publisher_t* publisher, urt_topic_t* topic, urt_message_t* message)
+void urtPublisherInit(urt_publisher_t* publisher, urt_topic_t* topic, urt_message_t* messages)
 {
+  urtDebugAssert(publisher);
+  urtDebugAssert(topic);
+
   urt_publisher_t.topic = topic;
-  #if (URT_CFG_PUBSUB_PROFILING)
-    urt_publisher_t.publishAttempts = 0;
-    urt_publisher_t.publishFails = 0;
-  endif /* URT_CFG_PUBSUB_PROFILING */
-  //add later: messages to contribute,...
+#if (URT_CFG_PUBSUB_PROFILING == true)
+  urt_publisher_t.publishAttempts = 0;
+  urt_publisher_t.publishFails = 0;
+#endif /* URT_CFG_PUBSUB_PROFILING */
+  if (messages != NULL) {
+    urtMutexLock(topic->lock);
+    urt_message_t* lastMessage = messages;
+    while (lastMessage->next != NULL) {
+      lastMessage = lastMessage->next;
+    }
+    //TODO: setnextpointer of last message to contribute to the message after the topic's latest message
+    topic->latestMessage->next = messages;
+  }
+
   return;
 }
 
@@ -78,4 +90,67 @@ void urtPublisherInit(urt_publisher_t* publisher, urt_topic_t* topic, urt_messag
  *
  * @return  Returns URT_STATUS_OK on success. Returns URT_STATUS_PUBLISH_TIMEOUT on timeout.
  */
-urt_status_t urtPublisherPublish(urt_publisher_t* publisher, void* payload, size_t bytes, urt_osTime_t t, urt_delay_t timeout) {return URT_STATUS_OK;}
+urt_status_t urtPublisherPublish(urt_publisher_t* publisher, void* payload, size_t bytes, urt_osTime_t t, urt_delay_t timeout)
+{
+  urtMutexLock(publisher->topic->lock);
+  urt_message_t* message = publisher->topic->next->latestMessage;
+  urt_osTime_t messageTime =  message->originTime;
+#if (URT_CFG_PUBSUB_PROFILING == true)
+  publisher->publishAttempts++;
+#endif /* URT_CFG_PUBSUB_PROFILING */
+
+  while (message->numHrtConsumersLeft > 0) {
+    urt_osCondvarWaitStatus_t timeout = urtCondvarWait(publisher->topic->hrtReleased, publisher->topic->lock, URT_DELAY_INFINITE);
+    if (timeout == URT_CONDVAR_WAITSTATUS_TIMEOUT) {
+#if (URT_CFG_PUBSUB_PROFILING == true)
+      publisher->publishFails++;
+#endif /* URT_CFG_PUBSUB_PROFILING */
+      urtMutexUnlock(publisher->topic->lock);
+      return URT_STATUS_PUBLISH_TIMEOUT;
+    }
+
+    if (messageTime != message->originTime) {
+      messageTime = message->originTime;
+      message = message->next;
+      while (message->originTime < messageTime) {
+        message = message->next;
+      }
+    }
+  }
+
+#if (URT_CFG_PUBSUB_PROFILING == true)
+  if (message->numHrtConsumersLeft > 0) { //TODO: never true because of while?
+    publisher->topic->numMessagesDiscarded++;
+  }
+#endif /* URT_CFG_PUBSUB_PROFILING */
+
+  publisher->topic->latestMessage = message; //TODO: Iterate topic's pointer to this message?
+  memcpy(message->payload, payload, bytes); //TODO: copy payload to message?
+  //TODO: Set origin time of message
+  publisher->topic->numHrtSubscribers = message->numHrtConsumersLeft;
+#if (URT_CFG_PUBSUB_PROFILING == true)
+  publisher->topic->numSubscribers = message->numHrtConsumersLeft; //TODO: Set number of comsumers?
+#endif /* URT_CFG_PUBSUB_PROFILING */
+
+#if (URT_CFG_PUBSUB_QOS_RATECHECKS == true)
+  publisher->topic->qosRateTimer =  NULL; //TODO: Set QoS rate timer wrt. most critical HRT Subscriber
+#endif /* URT_CFG_PUBSUB_QOS_RATECHECKS */
+
+#if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)
+  urt_hrtsubscriber_t* hrtSubscriber = publisher->topic->hrtSubscribers;
+  while (hrtSubscriber != NULL) {
+    if (!urtTimerIsArmed(hrtSubscriber->qosDeadlineTimer)) {
+      *hrtSubscriber->qosDeadlineTimer = message->originTime;
+    }
+    hrtSubscriber = hrtSubscriber->next;
+  }
+#endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */
+
+#if (URT_CFG_PUBSUB_PROFILING == true)
+  publisher->topic->numMessagesPublished++;
+#endif /* URT_CFG_PUBSUB_PROFILING */
+
+  //TODO: Fire event
+  urtMutexUnlock(publisher->topic->lock);
+  return URT_STATUS_OK;
+}
diff --git a/urtware.h b/urtware.h
index 46c38f8..99466e2 100644
--- a/urtware.h
+++ b/urtware.h
@@ -98,6 +98,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include <apps_urtosal.h>
 #include <urt_message.h>
 #include <urt_topic.h>
+#include <urt_publisher.h>
 #include <urt_subscriber.h>
 #include <urt_node.h>
 #include <urt_core.h>
