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/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;
+}
