From 299fdb28235fa2e8e7d1e8a9a4d81b48116e5567 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Thu, 22 Feb 2024 18:39:29 -0800
Subject: [PATCH] World: add gz-transport /world_control subscriber

Subscribe to WorldControl messages over gz-transport
(ZeroMQ) in addition to gazebo_transport (boost asio),
since gz-transport does a better job closing sockets.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 gazebo/physics/World.cc | 39 +++++++++++++++++++++++++++------------
 gazebo/physics/World.hh |  8 +++++++-
 2 files changed, 34 insertions(+), 13 deletions(-)

diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc
index 714be31478..6d36124fb0 100644
--- a/gazebo/physics/World.cc
+++ b/gazebo/physics/World.cc
@@ -270,6 +270,15 @@ void World::Load(sdf::ElementPtr _sdf)
                                            &World::OnFactoryMsg, this);
   this->dataPtr->controlSub = this->dataPtr->node->Subscribe("~/world_control",
                                            &World::OnControl, this);
+  {
+    // Also subscribe to WorldControl messages over ZeroMQ-based gz-transport
+    std::string worldControlTopic("/world_control");
+    if (!this->dataPtr->ignNode.Subscribe(worldControlTopic,
+                                          &World::OnWorldControl, this))
+    {
+      gzerr << "Error advertising topic [" << worldControlTopic << "]\n";
+    }
+  }
   this->dataPtr->playbackControlSub = this->dataPtr->node->Subscribe(
       "~/playback_control", &World::OnPlaybackControl, this);
 
@@ -1609,32 +1618,38 @@ void World::OnFactoryMsg(ConstFactoryPtr &_msg)
 //////////////////////////////////////////////////
 void World::OnControl(ConstWorldControlPtr &_data)
 {
-  if (_data->has_pause())
-    this->SetPaused(_data->pause());
+  this->OnWorldControl(*_data);
+}
+
+//////////////////////////////////////////////////
+void World::OnWorldControl(const msgs::WorldControl &_data)
+{
+  if (_data.has_pause())
+    this->SetPaused(_data.pause());
 
-  if (_data->has_step())
+  if (_data.has_step())
     this->OnStep();
 
-  if (_data->has_multi_step())
+  if (_data.has_multi_step())
   {
     // stepWorld is a blocking call so set stepInc directly so that world stats
     // will still be published
     this->SetPaused(true);
     std::lock_guard<std::recursive_mutex> lock(this->dataPtr->worldUpdateMutex);
-    this->dataPtr->stepInc = _data->multi_step();
+    this->dataPtr->stepInc = _data.multi_step();
   }
 
-  if (_data->has_seed())
+  if (_data.has_seed())
   {
-    ignition::math::Rand::Seed(_data->seed());
-    this->dataPtr->physicsEngine->SetSeed(_data->seed());
+    ignition::math::Rand::Seed(_data.seed());
+    this->dataPtr->physicsEngine->SetSeed(_data.seed());
   }
 
-  if (_data->has_reset())
+  if (_data.has_reset())
   {
     this->dataPtr->needsReset = true;
 
-    if (_data->reset().has_all() && _data->reset().all())
+    if (_data.reset().has_all() && _data.reset().all())
     {
       this->dataPtr->resetAll = true;
     }
@@ -1642,10 +1657,10 @@ void World::OnControl(ConstWorldControlPtr &_data)
     {
       this->dataPtr->resetAll = false;
 
-      if (_data->reset().has_time_only() && _data->reset().time_only())
+      if (_data.reset().has_time_only() && _data.reset().time_only())
         this->dataPtr->resetTimeOnly = true;
 
-      if (_data->reset().has_model_only() && _data->reset().model_only())
+      if (_data.reset().has_model_only() && _data.reset().model_only())
         this->dataPtr->resetModelOnly = true;
     }
   }
diff --git a/gazebo/physics/World.hh b/gazebo/physics/World.hh
index 2284318f01..c268e84aa7 100644
--- a/gazebo/physics/World.hh
+++ b/gazebo/physics/World.hh
@@ -538,10 +538,16 @@ namespace gazebo
       /// \brief Step callback.
       private: void OnStep();
 
-      /// \brief Called when a world control message is received.
+      /// \brief Called when a world control message is received on the
+      /// gazebo_transport topic using boost asio.
       /// \param[in] _data The world control message.
       private: void OnControl(ConstWorldControlPtr &_data);
 
+      /// \brief Called when a world control message is received on the
+      /// gz-transport topic using ZeroMQ.
+      /// \param[in] _data The world control message.
+      private: void OnWorldControl(const msgs::WorldControl &_data);
+
       /// \brief Called when log playback control message is received.
       /// \param[in] _data The log playback control message.
       private: void OnPlaybackControl(ConstLogPlaybackControlPtr &_data);