Skip to content

Commit

Permalink
Updating
Browse files Browse the repository at this point in the history
  • Loading branch information
nkoenig committed Apr 9, 2019
1 parent f7dd43a commit 650d174
Show file tree
Hide file tree
Showing 13 changed files with 54 additions and 20 deletions.
9 changes: 6 additions & 3 deletions examples/factory.ign
Original file line number Diff line number Diff line change
@@ -1,10 +1,13 @@
<?xml version="1.0"?>
<ignition version='1.0'>
<plugin name="ignition::launch::GazeboFactory"
filename="libignition-launch0-gazebo-factory.so">
<name>x2</name>
filename="libignition-launch0-gazebo-factory.so"
keepAlive="false">

<name>x3</name>
<allow_renaming>true</allow_renaming>
<uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/X2 UGV/1</uri>
<pose>0 2 0.063494 0 0 0</pose>
<pose>5 2 0.063494 0 0 0</pose>

<plugin entity_name="x2"
entity_type="model"
Expand Down
4 changes: 3 additions & 1 deletion include/ignition/launch/Plugin.hh
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,9 @@ namespace ignition
{
/// \brief Load function that each launch plugin must implement.
/// \param[in] _elem Pointer to the XML for this plugin.
public: virtual void Load(const tinyxml2::XMLElement *_elem) = 0;
/// \return True to keep the plugin alive. Return false to have the
/// plugin unloaded immediately.
public: virtual bool Load(const tinyxml2::XMLElement *_elem) = 0;
};

/// \brief Pointer to a launch plugin.
Expand Down
24 changes: 22 additions & 2 deletions plugins/gazebo_factory/GazeboFactory.cc
Original file line number Diff line number Diff line change
Expand Up @@ -30,13 +30,13 @@ GazeboFactory::GazeboFactory()
}

/////////////////////////////////////////////////
void GazeboFactory::Load(const tinyxml2::XMLElement *_elem)
bool GazeboFactory::Load(const tinyxml2::XMLElement *_elem)
{
const tinyxml2::XMLElement *elem;

msgs::EntityFactory req;

// Get the world file
// Get the uri
elem = _elem->FirstChildElement("uri");
if (elem)
{
Expand All @@ -45,6 +45,24 @@ void GazeboFactory::Load(const tinyxml2::XMLElement *_elem)
req.set_sdf(sdf);
}

// Get the name
elem = _elem->FirstChildElement("name");
if (elem)
req.set_name(elem->GetText());

// Get the pose
elem = _elem->FirstChildElement("pose");
if (elem)
{
ignition::math::Pose3d pose;
std::stringstream stream;
stream << elem->GetText();
stream >> pose;
std::cout << "Pose[" << pose << "]\n";
msgs::Set(req.mutable_pose(), pose);
}


unsigned int timeout = 2000;
msgs::Boolean rep;
bool result;
Expand Down Expand Up @@ -74,4 +92,6 @@ void GazeboFactory::Load(const tinyxml2::XMLElement *_elem)
}

igndbg << "Loaded GazeboFactory plugin.\n";

return false;
}
2 changes: 1 addition & 1 deletion plugins/gazebo_factory/GazeboFactory.hh
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ namespace ignition
public: virtual ~GazeboFactory() = default;

// Documentation inherited.
public: virtual void Load(
public: virtual bool Load(
const tinyxml2::XMLElement *_elem) override final;

private: transport::Node node;
Expand Down
6 changes: 4 additions & 2 deletions plugins/gazebo_gui/GazeboGui.cc
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ GazeboGui::~GazeboGui()
}

/////////////////////////////////////////////////
void GazeboGui::Load(const tinyxml2::XMLElement *_elem)
bool GazeboGui::Load(const tinyxml2::XMLElement *_elem)
{
int argc;
char **argv = nullptr;
Expand All @@ -51,7 +51,7 @@ void GazeboGui::Load(const tinyxml2::XMLElement *_elem)
if (!app->LoadConfig(configPath))
{
ignerr << "Unable to load GazeboGui config file[" << configPath << "]\n";
return;
return false;
}

// Customize window
Expand Down Expand Up @@ -116,4 +116,6 @@ void GazeboGui::Load(const tinyxml2::XMLElement *_elem)
// This blocks until the window is closed or we receive a SIGINT
this->app->exec();
this->app.reset();

return false;
}
2 changes: 1 addition & 1 deletion plugins/gazebo_gui/GazeboGui.hh
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ namespace ignition
public: virtual ~GazeboGui();

// Documentation inherited.
public: virtual void Load(
public: virtual bool Load(
const tinyxml2::XMLElement *_elem) override final;

/// \brief Run the GUI
Expand Down
3 changes: 2 additions & 1 deletion plugins/gazebo_server/GazeboServer.cc
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ GazeboServer::GazeboServer()
}

/////////////////////////////////////////////////
void GazeboServer::Load(const tinyxml2::XMLElement *_elem)
bool GazeboServer::Load(const tinyxml2::XMLElement *_elem)
{
gazebo::ServerConfig serverConfig;
const tinyxml2::XMLElement *elem;
Expand Down Expand Up @@ -144,4 +144,5 @@ void GazeboServer::Load(const tinyxml2::XMLElement *_elem)
this->server->Run(false, 0, !run);

igndbg << "Loaded GazeboServer plugin.\n";
return true;
}
2 changes: 1 addition & 1 deletion plugins/gazebo_server/GazeboServer.hh
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ namespace ignition
public: virtual ~GazeboServer() = default;

// Documentation inherited.
public: virtual void Load(
public: virtual bool Load(
const tinyxml2::XMLElement *_elem) override final;

/// \brief Private data pointer
Expand Down
4 changes: 3 additions & 1 deletion plugins/joy_to_twist/JoyToTwist.cc
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ JoyToTwist::~JoyToTwist()
}

/////////////////////////////////////////////////
void JoyToTwist::Load(const tinyxml2::XMLElement *_elem)
bool JoyToTwist::Load(const tinyxml2::XMLElement *_elem)
{
const tinyxml2::XMLElement *elem;

Expand Down Expand Up @@ -109,6 +109,8 @@ void JoyToTwist::Load(const tinyxml2::XMLElement *_elem)
<< " output_topic: " << this->outputTopic << std::endl;
this->running = true;
this->node.Subscribe(this->inputTopic, &JoyToTwist::OnJoy, this);

return true;
}

//////////////////////////////////////////////////
Expand Down
2 changes: 1 addition & 1 deletion plugins/joy_to_twist/JoyToTwist.hh
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ namespace ignition
public: virtual ~JoyToTwist();

// Documentation inherited
public: virtual void Load(
public: virtual bool Load(
const tinyxml2::XMLElement *_elem) override final;

/// \brief Run the plugin
Expand Down
6 changes: 4 additions & 2 deletions plugins/joystick/Joystick.cc
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ Joystick::~Joystick()
}

/////////////////////////////////////////////////
void Joystick::Load(const tinyxml2::XMLElement *_elem)
bool Joystick::Load(const tinyxml2::XMLElement *_elem)
{
// Get the name of the joystick device.
std::string deviceFilename = "/dev/input/js0";
Expand Down Expand Up @@ -161,7 +161,7 @@ void Joystick::Load(const tinyxml2::XMLElement *_elem)
{
std::cerr << "Unable to open joystick at [" << deviceFilename
<< "]. The joystick will not work.\n";
return;
return false;
}

this->unscaledDeadzone = 32767.0f * deadzone;
Expand All @@ -179,6 +179,8 @@ void Joystick::Load(const tinyxml2::XMLElement *_elem)
<< " dead_zone: " << deadzone << std::endl
<< " rate: " << intervalRate << std::endl
<< " accumulation_rate: " << accumulationRate << std::endl;

return true;
}

//////////////////////////////////////////////////
Expand Down
2 changes: 1 addition & 1 deletion plugins/joystick/Joystick.hh
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ namespace ignition
public: virtual ~Joystick();

// Documentation inherited
public: virtual void Load(
public: virtual bool Load(
const tinyxml2::XMLElement *_elem) override final;

/// \brief Run the plugin
Expand Down
8 changes: 5 additions & 3 deletions src/Manager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#include "Manager.hh"

using namespace ignition::launch;
using namespace std::chrono_literals;

/// \brief A class to encapsulate an executable (program) to run.
class Executable
Expand Down Expand Up @@ -233,7 +234,8 @@ bool Manager::RunConfig(const std::string &_config)
while (this->dataPtr->running && (!this->dataPtr->executables.empty() ||
!this->dataPtr->plugins.empty()))
{
this->dataPtr->runCondition.wait(lock);
std::cout << "Plugins[" << this->dataPtr->plugins.size() << "]\n";
this->dataPtr->runCondition.wait_for(lock, 1000ms);
}
this->dataPtr->running = false;

Expand Down Expand Up @@ -647,8 +649,8 @@ void ManagerPrivate::LoadPlugin(const tinyxml2::XMLElement *_elem)
<< "] File[" << file << "]" << std::endl;

PluginPtr plugin = loader.Instantiate(name);
plugin->QueryInterface<Plugin>()->Load(_elem);
this->plugins.insert(plugin);
if (plugin->QueryInterface<Plugin>()->Load(_elem))
this->plugins.insert(plugin);
}

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

0 comments on commit 650d174

Please sign in to comment.