WorldPluginLinkNode¶
Note
The WorldPluginLinkNode is written in C++ because Gazebo model plugins cannot be written in Python.
world_plugin_link.h¶
world_plugin_link.cpp¶
#include "world_plugin_link.h"
constexpr bool DEBUG = false;
// Spawn a new model from sdf string into the world.
void spawnModel(gazebo::physics::WorldPtr world, std_msgs::String msg) {
if (DEBUG) {
ROS_INFO_STREAM("Spawning model " << msg.data);
} else {
ROS_INFO_STREAM("Spawning model");
}
// Insert model from string
sdf::SDF modelSDF;
modelSDF.SetFromString(msg.data);
world->InsertModelSDF(modelSDF);
}
// Remove a model in the world.
void removeModel(gazebo::physics::WorldPtr world, std_msgs::String msg) {
if (DEBUG) {
ROS_INFO_STREAM("Removing model with name " << msg.data);
} else {
ROS_INFO_STREAM("Removing model");
}
world->RemoveModel(msg.data);
}
// Register this plugin with the simulator
GZ_REGISTER_WORLD_PLUGIN(WorldPluginLink)
WorldPluginLink::WorldPluginLink()
: node_handle_(ros::NodeHandle("~world_plugin_link")) {}
// Load the controller
void WorldPluginLink::Load(gazebo::physics::WorldPtr _parent, sdf::ElementPtr /*_sdf*/) {
// Make sure the ROS node for Gazebo has already been initalized
if (!ros::isInitialized()) {
ROS_FATAL_STREAM_NAMED("world_plugin_link",
"A ROS node for Gazebo has not been initialized, "
"unable to load plugin. "
<< "Load the Gazebo system plugin "
"'libgazebo_ros_api_plugin.so' in the "
"gazebo_ros package)");
return;
}
world = _parent;
std::string topic = "/simulation/gazebo/world/";
ROS_INFO_STREAM("Advertising world link topics in the namespace " << topic);
spawn_subscriber = node_handle_.subscribe(
topic + "/spawn_sdf_model", 1000, &WorldPluginLink::spawnModelCallback, this);
remove_subscriber = node_handle_.subscribe(
topic + "/remove_model", 1000, &WorldPluginLink::removeModelCallback, this);
}
void WorldPluginLink::spawnModelCallback(const std_msgs::String &msg) {
// Spawn new model inside a new thread.
std::thread task(spawnModel, world, msg);
task.detach();
}
void WorldPluginLink::removeModelCallback(const std_msgs::String &msg) {
// Remove a model inside a new thread.
std::thread task(removeModel, world, msg);
task.detach();
}