Skip to content

Commit

Permalink
Merge pull request #30 from ignitionrobotics/multi_spawn
Browse files Browse the repository at this point in the history 
Multi spawn
  • Loading branch information
@nkoenig
nkoenig authored May 20, 2020
2 parents e39c4c7 + 498992e commit 0469943
Show file tree
Hide file tree
Showing 6 changed files with 204 additions and 118 deletions.
4 changes: 2 additions & 2 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR)
#============================================================================
# Initialize the project
#============================================================================
project(ignition-launch1 VERSION 1.4.2)
project(ignition-launch1 VERSION 1.5.0)

#============================================================================
# Find ignition-cmake
Expand Down Expand Up @@ -77,7 +77,7 @@ ign_find_package (Qt5

#--------------------------------------
# Find ignition-gazebo
ign_find_package(ignition-gazebo2 REQUIRED VERSION 2.17
ign_find_package(ignition-gazebo2 REQUIRED VERSION 2.18
PRIVATE
COMPONENTS gui
)
Expand Down
5 changes: 5 additions & 0 deletions Changelog.md
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,10 @@
## Ignition Launch 1.x

### Ignition Launch 1.5.0 (2020-05-20)

1. Added support for spawning multiple entities in the same simulation step.
* [Pull Request 30](https://github.com/ignitionrobotics/ign-launch/pull/30)

### Ignition Launch 1.4.2 (2020-05-18)

1. Use the new GUI API of ign-gazebo. This adds support for saving worlds to SDFormat from the GUI.
Expand Down
42 changes: 22 additions & 20 deletions examples/factory.ign
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,27 +2,29 @@
<ignition version='1.0'>
<plugin name="ignition::launch::GazeboFactory"
filename="libignition-launch-gazebo-factory.so">
<name>x2</name>
<allow_renaming>true</allow_renaming>
<pose>1 2 0.5 0 0 0</pose>
<spawn>
<name>x2</name>
<allow_renaming>true</allow_renaming>
<pose>1 2 0.5 0 0 0</pose>

<sdf version='1.6'>
<include>
<uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/X2 UGV/1</uri>
<plugin filename="libignition-gazebo-diff-drive-system.so"
name="ignition::gazebo::systems::DiffDrive">
<left_joint>front_left_wheel_joint</left_joint>
<left_joint>rear_left_wheel_joint</left_joint>
<right_joint>front_right_wheel_joint</right_joint>
<right_joint>rear_right_wheel_joint</right_joint>
<wheel_separation>0.33559</wheel_separation>
<wheel_radius>0.098</wheel_radius>
</plugin>
<sdf version='1.6'>
<include>
<uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/X2 UGV/1</uri>
<plugin filename="libignition-gazebo-diff-drive-system.so"
name="ignition::gazebo::systems::DiffDrive">
<left_joint>front_left_wheel_joint</left_joint>
<left_joint>rear_left_wheel_joint</left_joint>
<right_joint>front_right_wheel_joint</right_joint>
<right_joint>rear_right_wheel_joint</right_joint>
<wheel_separation>0.33559</wheel_separation>
<wheel_radius>0.098</wheel_radius>
</plugin>

<!-- Publish robot state information -->
<plugin filename="libignition-gazebo-state-publisher-system.so"
name="ignition::gazebo::systems::StatePublisher"></plugin>
</include>
</sdf>
<!-- Publish robot state information -->
<plugin filename="libignition-gazebo-state-publisher-system.so"
name="ignition::gazebo::systems::StatePublisher"></plugin>
</include>
</sdf>
</spawn>
</plugin>
</ignition>
28 changes: 28 additions & 0 deletions examples/multi_factory.ign
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
<?xml version="1.0"?>
<ignition version='1.0'>
<plugin name="ignition::launch::GazeboFactory"
filename="libignition-launch-gazebo-factory.so">
<spawn>
<name>x2</name>
<allow_renaming>true</allow_renaming>
<pose>1 2 0.5 0 0 0</pose>

<sdf version='1.6'>
<include>
<uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/X3 UAV Config 1</uri>
</include>
</sdf>
</spawn>
<spawn>
<name>x1</name>
<allow_renaming>true</allow_renaming>
<pose>4 2 0.5 0 0 0</pose>

<sdf version='1.6'>
<include>
<uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/X1 Config 1</uri>
</include>
</sdf>
</spawn>
</plugin>
</ignition>
191 changes: 112 additions & 79 deletions plugins/gazebo_factory/GazeboFactory.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,52 +32,16 @@ GazeboFactory::GazeboFactory()
}

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

msgs::EntityFactory req;

// Set the sdf field, if an SDF string has been specified.
tinyxml2::XMLPrinter printer;
elem = _elem->FirstChildElement("sdf");
if (elem)
{
elem->Accept(&printer);
req.set_sdf(printer.CStr());
}

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

// Get <is_performer>, by default we assume a spawned model is a performer
bool isPerformer = true;
elem = _elem->FirstChildElement("is_performer");
if (elem)
{
std::string str = elem->GetText();
isPerformer = str == "1" || common::lowercase(str) == "true";
}

// Get <allow_renaming>
elem = _elem->FirstChildElement("allow_renaming");
if (elem)
{
std::string str = elem->GetText();
req.set_allow_renaming(str == "1" || common::lowercase(str) == "true");
}

// Get the pose
elem = _elem->FirstChildElement("pose");
if (elem)
// Get the sdf field, otherwise return.
const tinyxml2::XMLElement *sdfElem = _elem->FirstChildElement("sdf");
if (!sdfElem)
{
ignition::math::Pose3d pose;
std::stringstream stream;
stream << elem->GetText();
stream >> pose;
msgs::Set(req.mutable_pose(), pose);
// Return early if there is no <sdf> element.
return;
}

// Get a user-defined world name
Expand Down Expand Up @@ -112,7 +76,7 @@ bool GazeboFactory::Load(const tinyxml2::XMLElement *_elem)
{
ignerr << "No simulation worlds were found. Unable to run the factory. "
<< "Is Gazebo running?\n";
return false;
return;
}

// Warning if multiple worlds were found.
Expand All @@ -125,56 +89,125 @@ bool GazeboFactory::Load(const tinyxml2::XMLElement *_elem)
worldName = *worlds.begin();
}

unsigned int timeout = 2000;
msgs::Boolean rep;
bool result;
msgs::EntityFactory *req = this->worldFactoryMsgs[worldName].add_data();

std::string topic = "/world/";
topic += worldName + "/create";
tinyxml2::XMLPrinter printer;
sdfElem->Accept(&printer);
req->set_sdf(printer.CStr());

// Send the request.
bool executed = this->node.Request(topic, req, timeout, rep, result);
// Get <name>
elem = _elem->FirstChildElement("name");
if (elem)
req->set_name(elem->GetText());

if (executed && result && rep.data())
// Get <is_performer>, by default we assume a spawned model is a performer
bool isPerformer = true;
elem = _elem->FirstChildElement("is_performer");
if (elem)
{
igndbg << "Factory service call succeeded.\n";
if (isPerformer)
{
IGN_SLEEP_S(2);
topic = std::string("/world/") + worldName + "/level/set_performer";
msgs::StringMsg performerReq;
performerReq.set_data(req.name());
// \todo(anyone) Setting the size to 2,2,2 is a hack. Gazebo should
// calculate the bounding box based on the model information.
executed = this->node.Request(topic, performerReq, timeout, rep, result);

// msgs::Performer performerReq;
// performerReq.set_name(req.name());
// // \todo(anyone) Setting the size to 2,2,2 is a hack. Gazebo should
// // calculate the bounding box based on the model information.
// msgs::Set(performerReq.mutable_geometry()->mutable_box()->mutable_size(), math::Vector3d(2, 2, 2));
// executed = this->node.Request(topic, performerReq, timeout, rep, result);
}
std::string str = elem->GetText();
isPerformer = str == "1" || common::lowercase(str) == "true";
}
else
if (isPerformer)
this->worldPerformers[worldName].push_back(req->name());

// Get <allow_renaming>
elem = _elem->FirstChildElement("allow_renaming");
if (elem)
{
std::string str = elem->GetText();
req->set_allow_renaming(str == "1" || common::lowercase(str) == "true");
}

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

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

// Process each <spawn> ... </spawn> element.
elem = _elem->FirstChildElement("spawn");
while (elem)
{
if (executed)
this->ProcessSpawn(elem);
elem = elem->NextSiblingElement("spawn");
}

// Process the case where there is no <spawn> .. </spawn> entity, and only
// one "spawn" request is embedded in the plugin.
this->ProcessSpawn(_elem);

for (const auto &msg : this->worldFactoryMsgs)
{
unsigned int timeout = 2000;
msgs::Boolean rep;
bool result;

std::string topic = "/world/";
topic += msg.first + "/create_multiple";

// Send the request.
bool executed = this->node.Request(topic, msg.second, timeout, rep, result);

if (executed && result && rep.data())
{
if (result && !rep.data())
{
ignerr << "Factory service call completed, but returned a false value."
<< "You may have an invalid request. Check the configuration.\n";
}
else
igndbg << "Factory service call succeeded.\n";
if (!this->worldPerformers[msg.first].empty())
{
ignerr << "Factory service call failed.\n";
IGN_SLEEP_S(2);
topic = std::string("/world/") + msg.first + "/level/set_performer";

for (const auto &perf : this->worldPerformers[msg.first])
{
msgs::StringMsg performerReq;
performerReq.set_data(perf);
// \todo(anyone) Setting the size to 2,2,2 is a hack. Gazebo should
// calculate the bounding box based on the model information.
executed = this->node.Request(
topic, performerReq, timeout, rep, result);

// msgs::Performer performerReq;
// performerReq.set_name(req.name());
// // \todo(anyone) Setting the size to 2,2,2 is a hack. Gazebo should
// // calculate the bounding box based on the model information.
// msgs::Set(performerReq.mutable_geometry()->mutable_box()->mutable_size(), math::Vector3d(2, 2, 2));
// executed = this->node.Request(topic, performerReq, timeout, rep, result);
}
}
}
else
{
ignerr << "Factory service call timed out.\n";
if (executed)
{
if (result && !rep.data())
{
ignerr << "Factory service call completed, but returned a false value."
<< "You may have an invalid request. Check the configuration.\n";
}
else
{
ignerr << "Factory service call failed.\n";
}
}
else
{
ignerr << "Factory service call timed out.\n";
}
}
}



return false;
}
Loading

0 comments on commit 0469943

Please sign in to comment.