Building a Gazebo System Plugin for Multi‑Robot Coordination

Problem: Simulation lag when each robot runs its own plugin

When simulating dozens of robots, it is common to attach a separate model‑level plugin to each robot to handle sensor processing or low‑level control. Each plugin receives its own update callback, which adds overhead to the physics thread and can drive the real‑time factor below 1.0, making the simulation feel sluggish.

Thesis: A single System‑API plugin can coordinate multiple robots more efficiently

The Gazebo System API lets you hook into the global simulation lifecycle (pre‑physics, post‑physics, etc.) without modifying the core engine. By moving coordination logic into one System plugin, you reduce per‑robot callback frequency and keep heavy work off the physics step when possible.

Section 1: System API vs. per‑model plugins

Per‑model plugins inherit from gazebo::plugins::ModelPlugin and receive OnUpdate calls tied to a specific model’s update rate. A System plugin inherits from gazebo::system::System and implements methods such as PreUpdate, PostUpdate, or Configure. These methods are invoked once per simulation step for the entire world, giving you a natural place to iterate over all robots and apply shared logic.

Section 2: Designing a lightweight coordination plugin

We will create a plugin called MultiRobotCoordinator that:

1. Discovers all models with a specific <name> prefix (e.g., robot_*) during Configure.
2. Stores pointers to their Entity components.
3. In PostUpdate, reads each robot’s pose and writes a simple command (e.g., zero velocity) if they drift beyond a bound.
4. Keeps the per‑step work minimal: only a few Eigen vector operations per robot.

Section 3: Worked example

3.1 Source code (C++)

#include 
#include 
#include 

namespace gazebo {
class MultiRobotCoordinator : public System {
public:
  void Configure(const Entity &_entity,
                 const std::shared_ptr &_sdf,
                 EntityComponentManager &_ecm,
                 EventManager &/*_eventMgr*/) override {
    // Find all models whose name starts with "robot_"
    auto modelComp = _ecm.Component<components::Model>();
    for (auto ent : _ecm.EntitiesByComponents(modelComp)) {
      auto nameComp = _ecm.Component<components::Name>(ent);
      if (nameComp && nameComp->Data().find("robot_") == 0) {
        robotEntities.push_back(ent);
      }
    }
  }

  void PostUpdate(const UpdateInfo &/*_info",
                  const EntityComponentManager &_ecm) override {
    auto poseComp = _ecm.Component<components::Pose>();
    for (auto ent : robotEntities) {
      if (auto pose = poseComp.Component(ent)) {
        // Example: keep robots inside a 10m radius
        double dist = pose->Data().Pos().Length();
        if (dist > 10.0) {
          // Apply a corrective force via a simple velocity reset
          auto velComp = _ecm.Component<components::Velocity>(ent);
          if (velComp) velComp->Data() = math::Vector3d::Zero;
        }
      }
    }
  }

private:
  std::vector robotEntities;
};
}

// Register this plugin with the simulator
IGNITION_ADD_PLUGIN(gazebo::MultiRobotCoordinator,
                    gazebo::System)

3.2 Building the plugin

Assuming you have Gazebo Harmonic installed and a C++ compiler:

# Create a workspace directory
mkdir -p ~/gazebo_coordinator_ws/src
cd ~/gazebo_coordinator_ws/src
# Copy the source above into MultiRobotCoordinator.cpp
# Create a minimal CMakeLists.txt
cat > CMakeLists.txt <<'EOF'
cmake_minimum_required(VERSION 3.10)
find_package(ignition-gazebo6 REQUIRED)
add_library(MultiRobotCoordinator SHARED MultiRobotCoordinator.cpp)
target_link_libraries(MultiRobotCoordinator
    ${IGNITION-GAZEBO_LIBRARIES})
set_target_properties(MultiRobotCoordinator PROPERTies
    PREFIX "" OUTPUT_NAME "libMultiRobotCoordinator")
EOF
# Build
mkdir -p build && cd build
cmake ..
make -j$(nproc)
# The resulting library is libMultiRobotCoordinator.so

Run the commands in a terminal with normal user privileges; no sudo is required unless your Gazebo installation is in a system directory.

3.3 Loading the plugin via SDF

Create a simple world file empty_world.sdf that includes the plugin:

<sdf version="1.9">
  <world name="default">
    <plugin name="multi_robot_coordinator"
            filename="libMultiRobotCoordinator.so">
    </plugin>
    
    <include>
      <uri>model://robot</uri>
      <name>robot_0</name>
      <pose>0 0 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://robot</uri>
      <name>robot_1</name>
      <pose>2 0 0 0 0 0</pose>
    </include>
  </world>
</sdf>

Start Gazebo with verbose output to confirm the plugin loads:

cd ~/gazebo_coordinator_ws
# Ensure the library can be found
export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=$(pwd)/build:$IGN_GAZEBO_SYSTEM_PLUGIN_PATH
gz sim -s empty_world.sdf --verbose

Look for lines similar to:

[Msg] Loading plugin libMultiRobotCoordinator.so
[Msg] MultiRobotCoordinator configured

No output is invented; you should see the actual messages from your build.

3.4 Verifying the plugin does not cause lag

While the simulation runs, watch the console for the real‑time factor (RTF). A healthy simulation reports an RTF close to 1.0. If the RTF drops significantly (<0.8) after adding the plugin, consider:

• Reducing the frequency of heavy work (e.g., move logic to a timer callback).
• Profiling with gz topic -t /gazebo/default/performance_metrics to see where time is spent.

Practical check: run the simulation for 30 seconds, note the average RTF printed at the end, and compare it to a baseline run with the same world but without the System plugin.

Trade‑offs and limitations

While a System plugin reduces per‑model callback overhead, it introduces a single point of failure: an unhandled exception in PostUpdate can halt the entire simulation. Additionally, because the plugin runs on the physics thread, any blocking operation (e.g., file I/O, network calls) will directly impact the simulation step size and can cause instability. Keep the plugin’s workload lightweight and offload heavy computation to separate threads or ROS2 nodes if needed.

Actionable closing

If you are experiencing simulation slowdowns with many robots, try moving shared coordination logic into a System plugin as shown above. Build the plugin, load it via an SDF <plugin> tag, verify loading with --verbose, and monitor the real‑time factor to ensure you have not introduced overhead. Iterate by adjusting the plugin’s update frequency or moving costly work off the physics thread until the simulation runs smoothly.