Implement /server_control::stop (#1240)

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>

include/ignition/gazebo/Server.hh

@@ -89,6 +89,10 @@ namespace ignition
     ///   3. `/gazebo/resource_paths/add` : ignition::msgs::Empty
     ///     + Add new resource paths.
     ///
+    ///   4. `/server_control`(ignition::msgs::ServerControl) :
+    ///         ignition::msgs::Boolean
+    ///     + Control the simulation server.
+    ///
     /// ## Topics
     ///
     /// The following are topics provided by the Server.

include/ignition/gazebo/gui/TmpIface.hh

@@ -63,14 +63,14 @@ namespace ignition
       /// \param[in] _path Path to world file.
       public slots: void OnSaveWorldAs(const QString &_path);
 
-      /// \brief Server control service callback
-      /// This is the server-side logic which provides the world_control
-      /// service.
+      /// \brief This function does nothing and is kept only for retaining ABI
+      /// compatibility. /server_control service handling was moved to
+      /// ServerPrivate.
       /// \param[in] _req Request
       /// \param[out] _res Response
-      /// \return True for success
-      private: bool OnServerControl(const msgs::ServerControl &_req,
-                                          msgs::Boolean &_res);
+      /// \return False.
+      private: bool IGN_DEPRECATED(3) OnServerControl(
+        const msgs::ServerControl &_req, msgs::Boolean &_res);
 
       /// \brief Communication node
       private: transport::Node node;

src/ServerPrivate.cc

@@ -100,6 +100,10 @@ ServerPrivate::~ServerPrivate()
   {
     this->runThread.join();
   }
+  if (this->stopThread && this->stopThread->joinable())
+  {
+    this->stopThread->join();
+  }
 }
 
 //////////////////////////////////////////////////
@@ -419,6 +423,19 @@ void ServerPrivate::SetupTransport()
     ignerr << "Something went wrong, failed to advertise [" << pathTopic
            << "]" << std::endl;
   }
+
+  std::string serverControlService{"/server_control"};
+  if (this->node.Advertise(serverControlService,
+                           &ServerPrivate::ServerControlService, this))
+  {
+    ignmsg << "Server control service on [" << serverControlService << "]."
+           << std::endl;
+  }
+  else
+  {
+    ignerr << "Something went wrong, failed to advertise ["
+           << serverControlService << "]" << std::endl;
+  }
 }
 
 //////////////////////////////////////////////////
@@ -436,6 +453,49 @@ bool ServerPrivate::WorldsService(ignition::msgs::StringMsg_V &_res)
   return true;
 }
 
+//////////////////////////////////////////////////
+bool ServerPrivate::ServerControlService(
+  const ignition::msgs::ServerControl &_req, msgs::Boolean &_res)
+{
+  _res.set_data(false);
+
+  if (_req.stop())
+  {
+    if (!this->stopThread)
+    {
+      this->stopThread = std::make_shared<std::thread>([this]{
+        ignlog << "Stopping Gazebo" << std::endl;
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        this->Stop();
+      });
+    }
+    _res.set_data(true);
+  }
+
+  // TODO(chapulina): implement world cloning
+  if (_req.clone() || _req.new_port() != 0 || !_req.save_world_name().empty())
+  {
+    ignerr << "ServerControl::clone is not implemented" << std::endl;
+    _res.set_data(false);
+  }
+
+  // TODO(chapulina): implement adding a new world
+  if (_req.new_world())
+  {
+    ignerr << "ServerControl::new_world is not implemented" << std::endl;
+    _res.set_data(false);
+  }
+
+  // TODO(chapulina): implement loading a world
+  if (!_req.open_filename().empty())
+  {
+    ignerr << "ServerControl::open_filename is not implemented" << std::endl;
+    _res.set_data(false);
+  }
+
+  return true;
+}
+
 //////////////////////////////////////////////////
 void ServerPrivate::AddResourcePathsService(
     const ignition::msgs::StringMsg_V &_req)

src/ServerPrivate.hh

@@ -39,6 +39,8 @@
 
 #include <ignition/transport/Node.hh>
 
+#include <ignition/msgs/server_control.pb.h>
+
 #include "ignition/gazebo/config.hh"
 #include "ignition/gazebo/Export.hh"
 #include "ignition/gazebo/ServerConfig.hh"
@@ -114,6 +116,13 @@ namespace ignition
       /// \return True if successful.
       private: bool ResourcePathsService(ignition::msgs::StringMsg_V &_res);
 
+      /// \brief Callback for server control service.
+      /// \param[out] _req The control request.
+      /// \param[out] _res Whether the request was successfully fullfilled.
+      /// \return True if successful.
+      private: bool ServerControlService(
+        const ignition::msgs::ServerControl &_req, msgs::Boolean &_res);
+
       /// \brief A pool of worker threads.
       public: common::WorkerPool workerPool{2};
 
@@ -130,6 +139,9 @@ namespace ignition
       /// \brief Thread that executes systems.
       public: std::thread runThread;
 
+      /// \brief Thread that shuts down the system.
+      public: std::shared_ptr<std::thread> stopThread;
+
       /// \brief Our signal handler.
       public: ignition::common::SignalHandler sigHandler;
 

src/Server_TEST.cc

@@ -619,6 +619,64 @@ TEST_P(ServerFixture, SigInt)
   EXPECT_FALSE(*server.Running(0));
 }
 
+/////////////////////////////////////////////////
+TEST_P(ServerFixture, ServerControlStop)
+{
+  // Test that the server correctly reacts to requests on /server_control
+  // service with `stop` set to either false or true.
+
+  gazebo::Server server;
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  // Run forever, non-blocking.
+  server.Run(false, 0, false);
+
+  IGN_SLEEP_MS(500);
+
+  EXPECT_TRUE(server.Running());
+  EXPECT_TRUE(*server.Running(0));
+
+  transport::Node node;
+  msgs::ServerControl req;
+  msgs::Boolean res;
+  bool result{false};
+  bool executed{false};
+  int sleep{0};
+  int maxSleep{30};
+
+  // first, call with stop = false; the server should keep running
+  while (!executed && sleep < maxSleep)
+  {
+    igndbg << "Requesting /server_control" << std::endl;
+    executed = node.Request("/server_control", req, 100, res, result);
+    sleep++;
+  }
+  EXPECT_TRUE(executed);
+  EXPECT_TRUE(result);
+  EXPECT_FALSE(res.data());
+
+  IGN_SLEEP_MS(500);
+
+  EXPECT_TRUE(server.Running());
+  EXPECT_TRUE(*server.Running(0));
+
+  // now call with stop = true; the server should stop
+  req.set_stop(true);
+
+  igndbg << "Requesting /server_control" << std::endl;
+  executed = node.Request("/server_control", req, 100, res, result);
+
+  EXPECT_TRUE(executed);
+  EXPECT_TRUE(result);
+  EXPECT_TRUE(res.data());
+
+  IGN_SLEEP_MS(500);
+
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+}
+
 /////////////////////////////////////////////////
 TEST_P(ServerFixture, TwoServersNonBlocking)
 {

src/gui/TmpIface.cc

@@ -25,24 +25,16 @@ using namespace gazebo;
 /////////////////////////////////////////////////
 TmpIface::TmpIface()
 {
-  // Server control
-  this->node.Advertise("/server_control",
-      &TmpIface::OnServerControl, this);
 }
 
+
 /////////////////////////////////////////////////
-bool TmpIface::OnServerControl(const msgs::ServerControl &_req,
-                                     msgs::Boolean &_res)
+bool TmpIface::OnServerControl(const msgs::ServerControl &/*_req*/,
+                                     msgs::Boolean &/*_res*/)
 {
-  igndbg << "OnServerControl: request" << std::endl;
-  igndbg << _req.DebugString() << std::endl;
-
-  _res.set_data(true);
-
-  igndbg << "OnServerControl: response" << std::endl;
-  igndbg << _res.DebugString() << std::endl;
-
-  return true;
+  ignerr << "Called wrong /server_control callback. Call the one in "
+         << "ServerPrivate instead." << std::endl;
+  return false;
 }
 
 /////////////////////////////////////////////////