Skip to content

Commit

Permalink
Implement /server_control::stop (#1240)
Browse files Browse the repository at this point in the history
Signed-off-by: Martin Pecka <[email protected]>
  • Loading branch information
peci1 authored Dec 23, 2021
1 parent 41d4aba commit 0d00cc1
Show file tree
Hide file tree
Showing 6 changed files with 146 additions and 20 deletions.
4 changes: 4 additions & 0 deletions include/ignition/gazebo/Server.hh
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
12 changes: 6 additions & 6 deletions include/ignition/gazebo/gui/TmpIface.hh
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
60 changes: 60 additions & 0 deletions src/ServerPrivate.cc
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,10 @@ ServerPrivate::~ServerPrivate()
{
this->runThread.join();
}
if (this->stopThread && this->stopThread->joinable())
{
this->stopThread->join();
}
}

//////////////////////////////////////////////////
Expand Down Expand Up @@ -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;
}
}

//////////////////////////////////////////////////
Expand All @@ -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)
Expand Down
12 changes: 12 additions & 0 deletions src/ServerPrivate.hh
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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};

Expand All @@ -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;

Expand Down
58 changes: 58 additions & 0 deletions src/Server_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
20 changes: 6 additions & 14 deletions src/gui/TmpIface.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

/////////////////////////////////////////////////
Expand Down

0 comments on commit 0d00cc1

Please sign in to comment.