From c8bf695908b64c95041263900b2ca833e0bbfd69 Mon Sep 17 00:00:00 2001
From: Nick Lamprianidis <nlamprian@gmail.com>
Date: Wed, 30 Sep 2020 08:43:44 +0200
Subject: [PATCH] Improve transport::Publisher reliability (#2725)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 gazebo/transport/Publisher.cc             |  11 ++-
 test/regression/2724_failing_publisher.cc | 108 ++++++++++++++++++++++
 test/regression/CMakeLists.txt            |   1 +
 3 files changed, 116 insertions(+), 4 deletions(-)
 create mode 100644 test/regression/2724_failing_publisher.cc

diff --git a/gazebo/transport/Publisher.cc b/gazebo/transport/Publisher.cc
index cb5251e235..b05e8ed5f2 100644
--- a/gazebo/transport/Publisher.cc
+++ b/gazebo/transport/Publisher.cc
@@ -259,10 +259,13 @@ void Publisher::SendMessage()
           pubDataPtr, std::placeholders::_1), *pubIter);
 
       std::lock_guard<std::mutex> lock2(pubDataPtr->mutex);
-      if (result > 0)
-        pubDataPtr->pubIds[*pubIter] = result;
-      else
-        pubDataPtr->pubIds.erase(*pubIter);
+      if (pubDataPtr->pubIds.find(*pubIter) != pubDataPtr->pubIds.end())
+      {
+        if (result > 0)
+          pubDataPtr->pubIds[*pubIter] = result;
+        else
+          pubDataPtr->pubIds.erase(*pubIter);
+      }
     }
 
     // Clear the local buffer.
diff --git a/test/regression/2724_failing_publisher.cc b/test/regression/2724_failing_publisher.cc
new file mode 100644
index 0000000000..9c01eb3392
--- /dev/null
+++ b/test/regression/2724_failing_publisher.cc
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#ifndef _WIN32
+#include <unistd.h>
+#endif
+#include "gazebo/test/ServerFixture.hh"
+
+using namespace gazebo;
+using namespace gazebo::transport;
+
+class FailingPublisherTest : public ServerFixture
+{
+};
+
+class TestTransport : public CallbackHelper
+{
+  /////////////////////////////////////////////////
+  public: virtual bool HandleData(const std::string &/*_newdata*/,
+                                  boost::function<void(uint32_t)> _cb,
+                                  uint32_t _id) override
+  {
+    _cb(_id);
+    return true;
+  }
+
+  /////////////////////////////////////////////////
+  public: virtual bool HandleMessage(MessagePtr /*_newMsg*/) override
+  {
+    return true;
+  }
+
+  /////////////////////////////////////////////////
+  public: virtual bool IsLocal() const override
+  {
+    return false;
+  }
+};
+
+int g_receivedMsgs = 0;
+
+void RequestMsgCb(const ConstRequestPtr &/*_msg*/)
+{
+  ++g_receivedMsgs;
+}
+
+/////////////////////////////////////////////////
+TEST_F(FailingPublisherTest, DirectPublish)
+{
+  Load("worlds/empty.world");
+
+  // Initialize node
+  NodePtr node(new Node());
+  node->Init();
+
+  // Create publisher and subscriber
+  SubscriberPtr sub = node->Subscribe<msgs::Request>("~/request", &RequestMsgCb);
+  PublisherPtr pub = node->Advertise<msgs::Request>("~/request");
+
+  // Add a subscription in the topic publication
+  auto publication = TopicManager::Instance()->FindPublication(pub->GetTopic());
+  ASSERT_FALSE(publication == nullptr);
+  boost::shared_ptr<TestTransport> subscription(new TestTransport());
+  publication->AddSubscription(subscription);
+
+  // Publish messages
+  msgs::Request *msg = msgs::CreateRequest("entity_delete", "test_model");
+  pub->Publish(*msg, true);
+  pub->Publish(*msg, true);
+  delete msg;
+
+  // Wait for messages
+  bool success = false;
+  for (int i = 0; i < 500; ++i)
+  {
+    if (g_receivedMsgs == 2)
+    {
+      success = true;
+      break;
+    }
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  }
+
+  ASSERT_TRUE(success) << "Number of received messages is " << g_receivedMsgs;
+}
+
+/////////////////////////////////////////////////
+// Main
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/test/regression/CMakeLists.txt b/test/regression/CMakeLists.txt
index 3e19fce7b8..7220b042c5 100644
--- a/test/regression/CMakeLists.txt
+++ b/test/regression/CMakeLists.txt
@@ -43,6 +43,7 @@ set(tests
   2428_log_insertions.cc
   2430_revolute_joint_SetPosition.cc
   2505_revolute_joint_SetAxis.cc
+  2724_failing_publisher.cc
 )
 gz_build_tests(${tests} EXTRA_LIBS gazebo_test_fixture)