Add official Clover simulation config (#254)

* clover_description: Add preliminary configs/models

* clover_description: Use proper models for the drone

* clover_description: Be more specific about spawn arguments

* clover_description: Tweak parameters a bit, add collision boxes

* travis: Add .dae files to the list of ignored by eclint

* Add clover_simulation package

* clover: Add Gazebo plugin sources

* builder: Ignore clover_gazebo_plugins for actual drone

* clover_gazebo_plugins: Expose include directories for plugins

This should fix building the unit tests

* clover_gazebo_plugins: Remove dependency on gazebo_ros

This should prevent RPi image failing to build.

* travis, gitattributes: Mark clover_gazebo_plugins as vendored, stop checks

* clover_simulation: Minor package.xml fix

* clover_description: Add IMU joint preservation

Oh, Gazebo, you are ever so very helpful, it's hard to put my appreciation into words! If not for your helpful model simplification, I wouldn't have spent two hours looking through the plugin sources, the urdf sources, trying lots of
different options for the joints and links, and finally getting an answer from GazeboOverflow or however you've named your Q&A site. How wonderful it is to have an issue that makes you tear your hair out just because you know
what's better for me!

* clover_simulation: Add the bare necessities to run a simulation

* clover_gazebo_plugins: Prevent gazebo from trying to download models

For some reason the models are no longer available, so Gazebo just spends some time waiting for a timeout.

* clover_gazebo_plugins: Update Gazebo model database URI

* clover_simulation: Add script to find and launch PX4

* clover_simulation: Fix launch file

* clover_description: Add missing plugins

* simulation: Re-enable gazebo_ros dependencies

This will force rosdep to try to install gazebo_ros on the drone,
but this can be counteracted by --skip-keys rosdep option.
This does not look reliable, but I could not come up with a better
solution.

* builder: Be more resilient about apt-get errors

* builder: Remove reference to resolve_rosdep

* clover_description: Update Clover model, change xacro description

Previous xacro description file was not performing too well, so I went with
a more sensible route and started changing iris.xacro to use our Clover model.

* clover_description: Bring back constants.xacro

* clover_description: Prevent lumping for camera link/joint

* clover_description: Move near clipping plane further away

* clover_description: Allow setting width/height for rpi_cam

* clover_description: Add a Clover model with a camera

* clover_description: Remove whitespaces

* clover_description: Add drone+camera spawning .launch file

* clover_simulation: Add gazebo_ros here as well

* clover_simulation: Spawn drone with camera by default

* clover_simulation: Allow specifying data path for px4

* clover_simulation: Add startup scripts from px4

Big TODO: Clean them up eventually

* clover_simulation: Use local data files

* clover_simulation: Launch clover services by default

* clover_description: Depend on gazebo_plugins as well

libgazebo_ros_camera is in gazebo_plugins, so we need that package.

* clover_description: Fix camera_sensor description

* clover_description: Fix typo in package.xml

* clover_simulation: rename sim_gazebo.launch to simulator.launch

* clover_simulation: Don't look for ROMFS in px4_source_path

We provide our own, no reason to fail if we can't find the originals.

* clover_simulation: Remove extra CMakeLists.txt

* clover_description: Use xacro: namespace for xacro macros

* clover_description: Fix package.xml formatting

* clover_description: Better camera defaults

* clover_description: Add distance sensor

* clover_description: Add leg colliders

* clover_simulation: Actually forward vehicle name

* clover_description: Revert adding additional colliders

Unfortunately, this breaks physics too much

* clover_description: Tweak drone physics, make it more bouncy

* clover_description: Don't spawn the drone inside the floor

* clover_description: Set rangefinder min range outside drone collider

* clover_simulation: Set default flow parameters for Clover

* clover_description: Update Clover 4 model

* clover_simulation: rename sim_gazebo.launch to simulator.launch (#233)

* clover_simulation: Add workaround for Gazebo crashes in VMware

* clover_simulation: Ignore .git for now

* clover: Add "simulated" argument

* clover_simulation: Start Gazebo early

* clover_gazebo_plugins: Remove unused files

* clover_description: Allow turning sensors on and off

* clover_description: Fix rangefinder creation

* Remove unneeded stuff and use PX4 from catkin workspace

* Remove clover_gazebo_plugins

* Rename arg simulated to simulator

* clover: Change target names to avoid clashing with PX4

* Fix

* clover_simulation: Re-add deleted comments

* Add loop model

* loop.material: use tabs instead of spaces

* loop model: don’t rotate by yaw

* loop.material: turn on alpha_blend

* Rename model loop to loop_line

* Add parquet plane model

* loop_line: fix description

* Set alpha_blend for loop_solid material

* Add square line model

* Add CATKIN_IGNORE to models directory

* Add LED strip Gazebo model

* Add hardcoded URDF LED strip

* clover_description: Add LED xarco model

clover_simulation: Implement LED visual plugin and controller

* clover_simulation: Make led plugin less chatty

* clover_simulation: Depend on led_msgs

This should allow the packages to be built in the proper order.

* clover_simulation: Support building against Kinetic

* clover_simulation: Don't build plugins if Gazebo is not installed

* clover_description: Get rid of "constants" file

* clover_description: Add README

* clover_simulation: Add README

* clover_simulation: Make parquet thicker

Otherwise the rangefinder beam goes right through it.

* docs: Start working on simulation articles

* docs: Start working on the simulation overview (en)

* Add launch-file for PX4 v1.8.2

* clover_simulation: Disable GPS, use EKF2 by default

Ideally we should be using LPE, but it is broken in PX4 1.10, and our need for a somewhat working simulator is higher than for a completely correct one.

* clover_simulation: Add experimental throttling camera

* clover_simulation: Add note about throttling camera

* clover_description: Remove unused file

* clover_simulation: Link against CameraPlugin

* clover_description: Add option to use throttling_camera

* Add clover.world

* clover_description: Add calculated inertial parameters

* simulator: change default world to clover.world

* clover_simulation: Start working on ArUco generation script

Port over aruco_gazebo_gen, add more options.
Does not modify the world right now.

* clover_simulation: Make LED plugin less chatty

* clover_description: Be more ROS-like in script naming

* clover_simulation: Implement model insertion to the world

* clover_simulation: Allow specifying output model dir

* clover_description: Don't use throttling camera by default

throttling camera is still a work-in-progress, there's no reason to
enable it by default.

* clover_simulation: Use proper script name in CMakeLists

This is what typically happens when I'm rushed.

* docs: Add instructions for VM setup (en)

* clover_simulation: Remove extra spaces

* docs: Describe simulation usage (en)

* clover_simulation: Remove led_strip

* docs/assets: Crunch sim image a bit

* clover: Bump VL53L1X version

For some reason, 0.0.2 is not installable on x86.

* docs/simulation: Fix capitalization

* Remove remnants of clover_gazebo_plugins

* Remove unneeded Clover 3 model

* Remove empty.world and asphalt_plane model

* Remove unused LED strip model

* Reduce images size

* Shortened simulator related urls

Co-authored-by: Oleg Kalachev <okalachev@gmail.com>

clover_simulation/src/clover_simulation/__init__.py

@@ -0,0 +1,93 @@
+from docopt import docopt
+from os import path
+from sys import stdout
+
+from .map_parser import parse
+from .marker import Marker, generate_markers
+
+from .world import load_world, add_model, save_world
+
+
+USAGE = '''
+
+aruco_gen - Script for ArUco map/model generation.
+  Generates ArUco models from their description and optionally
+  adds them to an existing Gazebo world.
+
+Usage:
+  aruco_gen [--offset-x=<m>] [--offset-y=<m>] [--offset-z=<m>]
+            [--offset-roll=<rad>] [--offset-pitch=<rad>] [--offset-yaw=<rad>]
+            [--dictionary=<id>] [--single-model]
+            [--source-world=<path>] [--inplace]
+            [--model-path=<path>]
+            <aruco_map_file>
+  aruco_gen -h | --help
+
+Options:
+  -h, --help             Show this screen.
+  --offset-x=<m>         X offset in meters [default: 0].
+  --offset-y=<m>         Y offset in meters [default: 0].
+  --offset-z=<m>         Z offset in meters [default: 0].
+  --offset-roll=<rad>    roll offset in radians [default: 0].
+  --offset-pitch=<rad>   pitch offset in radians [default: 0].
+  --offset-yaw=<rad>     yaw offset in radians [default: 0].
+  --dictionary=<id>      ArUco dictionary ID [default: 2].
+  --single-model         Generate a single model instead of individual
+                         markers.
+  --source-world=<path>  Path to existing Gazebo world.
+  --inplace              Modify source world.
+  --model-path=<path>    Folder where generated models will be saved
+                         [default: ~/.gazebo/models]
+  aruco_map_file         Full path to the ArUco map file
+
+'''
+
+
+def aruco_gen():
+    opts = docopt(USAGE)
+
+    dictionary_id = int(opts['--dictionary'])
+    mapfile = opts['<aruco_map_file>']
+    single_model = opts['--single-model']
+    source_world = opts['--source-world']
+    inplace = opts['--inplace']
+
+    off_x = float(opts['--offset-x'])
+    off_y = float(opts['--offset-y'])
+    off_z = float(opts['--offset-z'])
+    off_roll = float(opts['--offset-roll'])
+    off_pitch = float(opts['--offset-pitch'])
+    off_yaw = float(opts['--offset-yaw'])
+
+    model_base_path = path.expanduser(opts['--model-path'])
+
+    markers = parse(mapfile)
+
+    if single_model:
+        mapname = path.split(mapfile)[-1]
+        model_path = path.join(model_base_path, 'aruco_{}'.format(mapname.replace('.', '_')))
+        generate_markers(markers, model_path, dictionary_id=dictionary_id, map_source=mapname)
+    else:
+        for marker in markers:
+            model_name = 'aruco_{}_{}'.format(dictionary_id, marker.id_)
+            model_path = path.join(model_base_path, model_name)
+            generate_markers([Marker(marker.id_, marker.size, 0, 0, 0, 0, 0, 0)],
+                model_path, dictionary_id=dictionary_id)
+
+    if source_world is not None:
+        world_tree = load_world(source_world)
+        if single_model:
+            world_tree = add_model(world_tree, 'aruco_{}'.format(mapname.replace('.', '_')),
+                off_x, off_y, off_z,
+                off_roll, off_pitch, off_yaw)
+        else:
+            if (abs(off_roll) > 0.001) or (abs(off_pitch) > 0.001) or (abs(off_yaw) > 0.001):
+                raise NotImplementedError('Sorry, angular offsets are not currently implemented for multimodel generation')
+            for marker in markers:
+                world_tree = add_model(world_tree, 'aruco_{}_{}'.format(dictionary_id, marker.id_),
+                    off_x + marker.x, off_y + marker.y, off_z + marker.z,
+                    marker.roll, marker.pitch, marker.yaw)
+
+        output = open(source_world, 'w') if inplace else stdout
+
+        save_world(world_tree, output)

clover_simulation/src/clover_simulation/map_parser.py

@@ -0,0 +1,45 @@
+# ArUco map parser (should be kept in sync with aruco_pose)
+
+from .marker import Marker
+
+
+def _parse_line(line):
+    '''
+    Parse a line of map data, returning a Marker object if
+    parsing succeeded, or None if it failed.
+    '''
+    if line.startswith('#'):
+        return None
+    elems = line.split()
+    if len(elems) < 4:
+        return None
+    try:
+        id_ = int(elems[0])
+        size = float(elems[1])
+        x = float(elems[2])
+        y = float(elems[3])
+        z = float(elems[4]) if len(elems) > 4 else 0
+        yaw = float(elems[5]) if len(elems) > 5 else 0
+        pitch = float(elems[6]) if len(elems) > 6 else 0
+        roll = float(elems[7]) if len(elems) > 7 else 0
+    except:
+        print('Warning - marformed line: {}'.format(line, sys.exc_info()[0]))
+        return None
+    return Marker(id_, size, x, y, z, roll, pitch, yaw)
+
+
+def parse(map_path):
+    '''
+    Parse a map at a given path.
+
+    map_path: Path to the ArUco map file.
+
+    Returns a list of Marker objects.
+    '''
+    markers = []
+    with open(map_path, 'r') as map_contents:
+        for line in map_contents.readlines():
+            parser_result = _parse_line(line)
+            if parser_result is not None:
+                markers.append(parser_result)
+    return markers

clover_simulation/src/clover_simulation/marker.py

@@ -0,0 +1,163 @@
+# Marker object definition and creation
+
+from __future__ import print_function
+
+from collections import namedtuple
+from string import Template
+from os import makedirs, path
+
+import cv2
+import numpy as np
+import math
+
+Marker = namedtuple('Marker',
+    ['id_', 'size', 'x', 'y', 'z', 'roll', 'pitch', 'yaw'])
+
+
+MARKER_MODEL_CFG_TEMPLATE = Template('''
+<?xml version="1.0"?>
+<model>
+    <name>${model_name}</name>
+    <version>1.0</version>
+    <sdf version="1.6">${sdf_name}</sdf>
+    <author>
+        <name>Marker Generator script</name>
+        <email>marker@generator.sh</email>
+    </author>
+    <description>
+        ${model_name}
+    </description>
+</model>
+''')
+
+
+MARKER_VISUAL_TEMPLATE = Template('''
+<visual name="visual_marker_${marker_id}">
+    <pose>${x} ${y} ${z} ${roll} ${pitch} ${yaw}</pose>
+    <cast_shadows>false</cast_shadows>
+    <geometry>
+        <box>
+            <size>${marker_full_size} ${marker_full_size} 1e-3</size>
+        </box>
+    </geometry>
+    <material>
+        <script>
+            <uri>model://${model_directory}/materials/scripts</uri>
+            <uri>model://${model_directory}/materials/textures</uri>
+            <name>aruco/marker_${marker_id}</name>
+        </script>
+    </material>
+</visual>
+''')
+
+
+MARKER_MODEL_SDF_TEMPLATE = Template('''
+<?xml version="1.0"?>
+<sdf version="1.5">
+    <model name="${model_name}">
+        <static>true</static>
+        <link name="${model_name}_link">
+            ${model_visuals}
+        </link>
+    </model>
+</sdf>
+''')
+
+
+MARKER_MATERIAL_TEMPLATE = Template('''
+material aruco/marker_${marker_id}
+{
+    technique
+    {
+        pass
+        {
+            texture_unit
+            {
+                texture aruco_marker_${marker_id}.png
+                filtering none
+                scale 1.0 1.0
+            }
+        }
+    }
+}
+''')
+
+
+def model_name(model_directory):
+    '''
+    Extract model name from model directory.
+
+    model_directory: Full path to the model.
+
+    Returns Gazebo-compatible model name (available through model:// URI schema)
+    '''
+    return path.split(model_directory)[1]
+
+
+def generate_markers(markers, model_directory, dictionary_id=2, map_source=''):
+    '''
+    Generate markers from a list. Result is a single Gazebo
+    model with all markers inside it.
+
+    markers: A List-like object containing Marker objects.
+    model_directory: Target directory for the model.
+    dictionary_id: Predefined ArUco dictionary ID, as defined by OpenCV.
+    map_source: An optional string with the name of the ArUco map file.
+    '''
+    script_directory = path.join(model_directory, 'materials', 'scripts')
+    texture_directory = path.join(model_directory, 'materials', 'textures')
+
+    try:
+        if not path.exists(script_directory):
+            makedirs(script_directory)
+        if not path.exists(texture_directory):
+            makedirs(texture_directory)
+    except:
+        print('Could not create material/texture directories!')
+
+    aruco_dict = cv2.aruco.getPredefinedDictionary(dictionary_id)
+    marker_bits = aruco_dict.markerSize
+    marker_outer_bits = marker_bits + 2 # "black border" bits
+    marker_border_bits = marker_bits + 4 # "white border" bits
+    materials = []
+    visuals = []
+
+    for marker in markers:
+        texture_name = 'aruco_marker_{}_{}.png'.format(dictionary_id, marker.id_)
+
+        marker_image = np.zeros((marker_border_bits, marker_border_bits), dtype=np.uint8)
+        marker_image[:,:] = 255
+        marker_image[1:marker_border_bits - 1, 1:marker_border_bits - 1] = cv2.aruco.drawMarker(
+            aruco_dict, marker.id_, marker_outer_bits)
+        cv2.imwrite(path.join(texture_directory, texture_name), marker_image)
+        materials.append(MARKER_MATERIAL_TEMPLATE.substitute(
+            marker_id='{}_{}'.format(dictionary_id, marker.id_)
+        ))
+        visuals.append(MARKER_VISUAL_TEMPLATE.substitute(
+            marker_id='{}_{}'.format(dictionary_id, marker.id_),
+            x=marker.x,
+            y=marker.y,
+            z=marker.z,
+            roll=marker.roll,
+            pitch=marker.pitch,
+            yaw=marker.yaw + (math.pi / 2),
+            model_directory=model_name(model_directory),
+            marker_full_size=marker.size * marker_border_bits / marker_outer_bits
+        ))
+
+    with open(path.join(script_directory, 'aruco_materials.material'), 'w') as f:
+        f.write(''.join(materials))
+
+    with open(path.join(model_directory, 'aruco_model.sdf'), 'w') as f:
+        f.write(MARKER_MODEL_SDF_TEMPLATE.substitute(
+            model_name='aruco_{}_{}'.format(dictionary_id, len(markers)),
+            model_visuals=''.join(visuals)
+        ))
+
+    with open(path.join(model_directory, 'model.config'), 'w') as f:
+        f.write(MARKER_MODEL_CFG_TEMPLATE.substitute(
+            model_name='ArUco {} (dictionary {})'.format(
+                markers[0].id_ if len(markers) == 1 else 'field from ' + map_source,
+                dictionary_id),
+            sdf_name='aruco_model.sdf'
+        ))

clover_simulation/src/clover_simulation/world.py

@@ -0,0 +1,43 @@
+# Gazebo world changer
+
+import xml.etree.ElementTree as ET
+
+from string import Template
+
+WORLD_INCLUDE = Template('''
+<include>
+    <uri>model://${model_name}</uri>
+    <pose>${x} ${y} ${z} ${roll} ${pitch} ${yaw}</pose>
+</include>
+''')
+
+def load_world(world_file):
+    '''
+    Load Gazebo world as an ElementTree object
+    '''
+    return ET.parse(world_file)
+
+
+def add_model(world, model_name, x, y, z, roll, pitch, yaw, index=0):
+    '''
+    Create and add an element to the world
+    '''
+    world_elem = world.find('world')
+    model_elem = ET.fromstring(WORLD_INCLUDE.substitute(
+        model_name=model_name,
+        x=x,
+        y=y,
+        z=z,
+        roll=roll,
+        pitch=pitch,
+        yaw=yaw
+    ))
+    world_elem.insert(index, model_elem)
+    return world
+
+
+def save_world(world, file):
+    '''
+    Save the world to file-like object
+    '''
+    return world.write(file)

clover_simulation/src/sim_leds.cpp

@@ -0,0 +1,232 @@
+#include <led_msgs/SetLEDs.h>
+#include <led_msgs/LEDStateArray.h>
+
+#include <ros/ros.h>
+#include <ros/callback_queue.h>
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/rendering/rendering.hh>
+
+#if GAZEBO_MAJOR_VERSION >= 9
+#include <ignition/math/Color.hh>
+using GazeboColor = ignition::math::Color;
+#else
+#include <gazebo/common/Color.hh>
+using GazeboColor = gazebo::common::Color;
+#endif
+
+#include <string>
+#include <unordered_map>
+#include <mutex>
+#include <vector>
+
+#include <thread>
+
+namespace sim_led
+{
+// Forward declaration of the plugin for led controller
+class LedVisualPlugin;
+} // 
+
+namespace led_controller
+{
+
+/// LedController: a class that provides ROS interface for the LEDs.
+class LedController
+{
+private:
+	/// Role for the current controller
+	enum class Role
+	{
+		Client, // Client: runs on /gazebo_gui, responsible for "preview window"
+		Server  // Server: runs on /gazebo, responsible for renders on Gazebo sensors
+	} role;
+
+	// Pointers to the LED plugins that we know about
+	std::vector<sim_led::LedVisualPlugin*> registeredLeds;
+	// Mutex protecting the vector
+	std::mutex registryMutex;
+	std::string robotNamespace;
+
+	std::unique_ptr<ros::NodeHandle> nh;
+
+	ros::ServiceServer setLedsSrv;
+	// Note: LED state should only be published by the /gazebo node
+	led_msgs::LEDStateArray ledState;
+	ros::Publisher statePublisher;
+	// LED state will be read from the topic to avoid creating more services
+	ros::Subscriber stateSubscriber;
+
+	bool setLeds(led_msgs::SetLEDs::Request& req, led_msgs::SetLEDs::Response& resp);
+	void handleLedsMsg(const led_msgs::LEDStateArrayConstPtr& leds);
+
+public:
+	LedController(const std::string& robotNamespace) : robotNamespace(robotNamespace)
+	{
+		// We need "libgazebo_ros_api.so" to be loaded
+		if (!ros::isInitialized())
+		{
+			ROS_FATAL_NAMED("LedController", "Tried to load ROS plugin when ROS Gazebo API is not loaded. Please use gazebo_ros node to"
+			                                 "launch Gazebo.");
+		}
+
+		role = (ros::this_node::getName() == "/gazebo") ? Role::Server : Role::Client;
+		ROS_INFO_NAMED(("LedController_" + robotNamespace).c_str(), "LedController has started (as %s)", role == Role::Client ? "client" : "server");
+
+		nh.reset(new ros::NodeHandle(robotNamespace));
+		if (role == Role::Server)
+		{
+			setLedsSrv = nh->advertiseService("led/set_leds", &LedController::setLeds, this);
+			statePublisher = nh->advertise<led_msgs::LEDStateArray>("led/state", 1, true);
+		}
+		else
+		{
+			// LED state should be published to the "led/state" topic, so we grab our data there
+			stateSubscriber = nh->subscribe<led_msgs::LEDStateArray>("led/state", 1, &LedController::handleLedsMsg, this);
+		}
+	};
+
+	~LedController()
+	{
+		nh->shutdown();
+	}
+
+	void registerPlugin(sim_led::LedVisualPlugin* plugin, int ledIdx = 0, int totalLeds = 0)
+	{
+		assert(ledIdx < totalLeds);
+		std::lock_guard<std::mutex> lock(registryMutex);
+		if (totalLeds > 0) {
+			registeredLeds.resize(totalLeds);
+			ledState.leds.resize(totalLeds);
+		}
+		ROS_DEBUG_NAMED(("LedController_" + robotNamespace).c_str(), "Registering LED visual plugin to %s (LED id=%d)", (role == Role::Client) ? "client" : "server", ledIdx);
+		registeredLeds[ledIdx] = plugin;
+		ledState.leds[ledIdx].index = ledIdx;
+		if (role == Role::Server)
+			statePublisher.publish(ledState);
+	}
+
+	void unregisterPlugin(sim_led::LedVisualPlugin* plugin)
+	{
+		std::lock_guard<std::mutex> lock(registryMutex);
+		auto it = std::find(registeredLeds.begin(), registeredLeds.end(), plugin);
+		if (it != registeredLeds.end())
+		{
+			ROS_DEBUG_STREAM_NAMED(("LedController_" + robotNamespace).c_str(), "Unregistering LED visual plugin (LED id=" << (it - registeredLeds.begin()) << ")");
+			*it = nullptr;
+		}
+	}
+};
+
+// Guards controllers map (static to led_controller::get())
+std::mutex controllerMutex;
+
+LedController& get(std::string robotNamespace)
+{
+	static std::unordered_map<std::string, std::unique_ptr<LedController>> controllers;
+	std::lock_guard<std::mutex> lock(controllerMutex);
+	auto it = controllers.find(robotNamespace);
+	if (it == controllers.end()) {
+		gzwarn << "Creating new LED controller for namespace " << robotNamespace << "\n";
+		controllers[robotNamespace].reset(new LedController(robotNamespace));
+		return *controllers[robotNamespace];
+	}
+	return *(it->second);
+}
+
+} // led_controller
+
+
+namespace sim_led
+{
+
+class LedVisualPlugin : public gazebo::VisualPlugin {
+private:
+	std::string ns;
+	gazebo::rendering::VisualPtr vptr;
+
+public:
+	void Load(gazebo::rendering::VisualPtr visual, sdf::ElementPtr sdf) override {
+		// FIXME: This is a fragile way to guess our index
+		// FIXME: This is based on an assumption that the parent will have a mangled name
+		// FIXME: (like led_strip::base_link::base_link_fixed_joint_lump__led_00_link_visual_1)
+		// FIXME: This will obviously break if gazebo decides to go with something else
+		auto parentName = sdf->GetParent()->GetAttribute("name")->GetAsString();
+		auto lastDashPos = parentName.rfind('_');
+		int myIndex = 0;
+		if (lastDashPos != std::string::npos)
+		{
+			auto indexStr = parentName.substr(lastDashPos + 1);
+			try {
+				myIndex = std::stoi(indexStr);
+			} catch(const std::exception &e) {
+				gzwarn << "Failed to convert " << indexStr << " to integer: " << e.what() <<  ", assuming 0\n";
+				myIndex = 0;
+			}
+		}
+		else
+		{
+			gzerr << "Failed to parse parent name: " << parentName << "; could not determine our index\n";
+		}
+
+		ns = "";
+		if (sdf->HasElement("robotNamespace")) {
+			ns = sdf->Get<std::string>("robotNamespace");
+		}
+		if (!sdf->HasElement("ledCount")) {
+			gzerr << "ledCount is not set, but is required for the plugin to function correctly\n";
+			return;
+		}
+		int totalLeds = sdf->Get<int>("ledCount");
+
+		vptr = visual;
+		led_controller::get(ns).registerPlugin(this, myIndex, totalLeds);
+	}
+
+	~LedVisualPlugin()
+	{
+		led_controller::get(ns).unregisterPlugin(this);
+	}
+
+	void SetColor(const GazeboColor& emissive)
+	{
+		vptr->SetEmissive(emissive);
+	}
+
+};
+}
+
+// FIXME: These two functions basically do the same thing, maybe they can be merged?
+bool led_controller::LedController::setLeds(led_msgs::SetLEDs::Request &req, led_msgs::SetLEDs::Response &resp)
+{
+	std::lock_guard<std::mutex> lock(registryMutex);
+	for(const auto& led : req.leds)
+	{
+		if (led.index < registeredLeds.size()) {
+			auto color = GazeboColor(led.r / 255.0f, led.g / 255.0f, led.b / 255.0f);
+			auto ledPlugin = registeredLeds[led.index];
+			if (ledPlugin) ledPlugin->SetColor(color);
+			ledState.leds[led.index].r = led.r;
+			ledState.leds[led.index].g = led.g;
+			ledState.leds[led.index].b = led.b;
+		}
+	}
+	statePublisher.publish(ledState);
+	resp.success = true;
+	return true;
+}
+
+void led_controller::LedController::handleLedsMsg(const led_msgs::LEDStateArrayConstPtr& leds)
+{
+	std::lock_guard<std::mutex> lock(registryMutex);
+	for(const auto& led : leds->leds)
+	{
+		if (led.index < registeredLeds.size())
+		{
+			auto color = GazeboColor(led.r / 255.0f, led.g / 255.0f, led.b / 255.0f);
+			auto ledPlugin = registeredLeds[led.index];
+			if (ledPlugin) ledPlugin->SetColor(color);
+		}
+	}
+}
+
+GZ_REGISTER_VISUAL_PLUGIN(sim_led::LedVisualPlugin);

clover_simulation/src/throttling_camera.cpp

@@ -0,0 +1,275 @@
+#include <gazebo/plugins/CameraPlugin.hh>
+// physics definitions and functions: Required to perform slowdown
+#include <gazebo/physics/physics.hh>
+// ROS simulated camera class with most of the boilerplate already in place
+#include <gazebo_plugins/gazebo_ros_camera_utils.h>
+#include <ros/ros.h>
+#include <memory>
+
+#include <deque>
+#include <algorithm>
+
+namespace
+{
+/**
+ * Simple statistics-collecting class.
+ */
+class AverageStat
+{
+private:
+	/** Currently collected samples. */
+	std::deque<double> samples;
+	/** Number of samples to store (also, the number of samples considered "adequate") */
+	size_t maxSamples;
+	/** Currently stored average value */
+	double average = 0;
+	/** Currently stored standard deviation value */
+	double stdev = 0;
+	/** Largest standard deviation that is considered adequate */
+	double maxStDev;
+
+public:
+	AverageStat(size_t numSamples, double validStDev) :
+		samples(),
+		maxSamples(numSamples),
+		maxStDev(validStDev)
+	{}
+
+	/**
+	 * Add a sample and recalculate average and standard deviation.
+	 * 
+	 * @param sample New sampled value.
+	 * @return New average value.
+	 */
+	double update(double sample)
+	{
+		samples.push_back(sample);
+		if (samples.size() > maxSamples)
+		{
+			samples.pop_front();
+		}
+		average = std::accumulate(samples.begin(), samples.end(), 0.0) / samples.size();
+		double stdevSquared = std::accumulate(samples.begin(), samples.end(), 0.0,
+			[&](double sum, double xi) {
+				return sum + (xi - average) * (xi - average);
+			}) / samples.size();
+		stdev = std::sqrt(stdevSquared);
+		return average;
+	}
+
+	/**
+	 * Get current average value of all samples.
+	 * 
+	 * @note This function will return a result even if it is not considered valid.
+	 */
+	double getAverage() const
+	{
+		return average;
+	}
+
+	/**
+	 * Get current standard deviation of all samples.
+	 * 
+	 * @note This function will return a result even if it is not considered valid.
+	 */
+	double getStDev() const
+	{
+		return stdev;
+	}
+
+	/**
+	 * Check if the average value is considered "adequate".
+	 * 
+	 * @return True if the number of samples is not less than required and standard deviation is within limits, false otherwise
+	 */
+	bool isAdequate() const
+	{
+		return (samples.size() >= maxSamples) && (stdev < maxStDev);
+	}
+
+	/**
+	 * Drop all samples and start anew.
+	 * 
+	 * @note This does not actually change average and stdev, but the stats are considered inadequate after this call.
+	 */
+	void reset()
+	{
+		samples.clear();
+	}
+};
+
+}
+
+namespace throttling_camera
+{
+
+/**
+ * Gazebo camera plugin for a world-throttling (rate-preserving) camera.
+ * 
+ * This plugin will slow the simulation down to maintain average publishing rate. Note that
+ * this plugin will *only* perform slowdown, it *will not* speed the simulation back up!
+ */
+class ThrottlingCamera : public gazebo::CameraPlugin, gazebo::GazeboRosCameraUtils
+{
+private:
+	/** A pointer to the Gazebo camera sensor. */
+	gazebo::sensors::SensorPtr camPtr;
+	/** A pointer to the current simulated world (required to change world parameters) */
+	gazebo::physics::WorldPtr world;
+	/** A pointer to the physics preset manager. Used to actually slow the simulation down. */
+	gazebo::physics::PresetManagerPtr presetManager;
+
+	/** Maximum update interval that is considered "okay". Should be higher than the "average" update interval to avoid extreme slowdowns */
+	double maxUpdateInterval;
+
+	/** Statistics for publishing time intervals. */
+	std::unique_ptr<AverageStat> timeSamples;
+public:
+	ThrottlingCamera() = default;
+	~ThrottlingCamera() override = default;
+
+	/**
+	 * Plugin load function. Called by Gazebo each time the plugin is instantiated.
+	 * 
+	 * @param parent Gazebo sensor that this plugin connects to.
+	 * @param sdf SDF element containing this plugin.
+	 */
+	void Load(gazebo::sensors::SensorPtr parent, sdf::ElementPtr sdf) override
+	{
+		if (!ros::isInitialized())
+		{
+			ROS_FATAL_NAMED("throttling_camera", "ROS node for Gazebo has not been initialized, unable to load plugin");
+			return;
+		}
+		ROS_DEBUG_NAMED("throttling_camera", "Initializing ROS throttling (stable-rate) camera");
+
+		CameraPlugin::Load(parent, sdf);
+
+		world = gazebo::physics::get_world(parent->WorldName());
+#if GAZEBO_MAJOR_VERSION >= 8
+		presetManager = world->PresetMgr();
+#else
+		presetManager = world->GetPresetManager();
+#endif /* GAZEBO_MAJOR_VERSION */
+
+		// Same as in PX4
+		if (presetManager->CurrentProfile() != "default_physics")
+		{
+			gzwarn << "Current physics profile is not default_physics, but actually is " << presetManager->CurrentProfile() << "\n";
+			if (!presetManager->CurrentProfile("default_physics"))
+			{
+				gzerr << "Could not set current profile to default_physics!\n";
+			}
+		}
+
+		double minUpdateRate = parent->UpdateRate();
+		if (sdf->HasElement("minUpdateRate"))
+		{
+			minUpdateRate = sdf->Get<double>("minUpdateRate");
+		}
+		maxUpdateInterval = 1.0 / minUpdateRate;
+
+		size_t windowSize = 10;
+		if (sdf->HasElement("windowSize"))
+		{
+			windowSize = sdf->Get<size_t>("windowSize");
+		}
+
+		double maxStDev = 0.02;
+		if (sdf->HasElement("maxStDev"))
+		{
+			maxStDev = sdf->Get<double>("maxStDev");
+		}
+
+		timeSamples.reset(new AverageStat(windowSize, maxStDev));
+
+		camPtr = parent;
+
+		parentSensor_ = camPtr;
+		width_ = width;
+		height_ = height;
+		depth_ = depth;
+		format_ = format;
+		camera_ = camera;
+
+		gazebo::GazeboRosCameraUtils::Load(parent, sdf);
+	}
+
+	/**
+	 * Frame callback. Called every time a new frame is rendered by the camera.
+	 * 
+	 * Checks whether we should slow simulation down and publishes a new image
+	 * message.
+	 * 
+	 * @param image Image data.
+	 * @param width Image width, in pixels.
+	 * @param height Image height, in pixels.
+	 * @param depth Image depth, in bytes.
+	 * @param format Image format description string.
+	 */
+	void OnNewFrame(const unsigned char *image, unsigned int width, unsigned int height, unsigned int depth,
+	                const std::string &format) override {
+
+		// Note: sensorUpdateTime uses simulated time
+		auto sensorUpdateTime = camPtr->LastMeasurementTime();
+		// If sensor was not active for some reason, we allow it to get new data on next frame
+		if (!camPtr->IsActive())
+		{
+			camPtr->SetActive(true);
+			last_update_time_ = sensorUpdateTime;
+			timeSamples->reset();
+			return;
+		}
+
+		boost::mutex::scoped_lock lock(*image_connect_count_lock_);
+		if (*image_connect_count_ > 0)
+		{
+			if (sensorUpdateTime < last_update_time_)
+			{
+				ROS_WARN_NAMED("throttling_camera", "Negative sensor update time difference (world reset?)");
+				last_update_time_ = sensorUpdateTime;
+				timeSamples->reset();
+			}
+
+			auto timeDelta = sensorUpdateTime - last_update_time_;
+			timeSamples->update(timeDelta.Double());
+
+			// We want to throttle the simulation down if we have measurements too far apart
+			if (timeSamples->isAdequate() && timeSamples->getAverage() > maxUpdateInterval)
+			{
+				ROS_INFO_STREAM_NAMED("throttling_camera", "Had average update period of "
+										<< timeSamples->getAverage() << " (stdev: " << timeSamples->getStDev() <<  ")"
+										<< ", but desired update period is " << update_period_
+										<< ", throttling simulation down");
+				boost::any currentRealTimeUpadteRateParam;
+				if (!presetManager->GetCurrentProfileParam("real_time_update_rate", currentRealTimeUpadteRateParam))
+				{
+					gzerr << "Failed to get real time update rate parameter!\n";
+				}
+				auto currentRate = boost::any_cast<double>(currentRealTimeUpadteRateParam);
+				// We are being somewhat aggressive here, maybe we could throttle the world
+				// down in steps?
+				double slowdownFactor = update_period_ / timeSamples->getAverage();
+				auto nextRate = currentRate * slowdownFactor;
+				if (!presetManager->SetCurrentProfileParam("real_time_update_rate", nextRate))
+				{
+					gzerr << "Failed to set real time update rate parameter!\n";
+				}
+				if (slowdownFactor < 0.5)
+				{
+					ROS_WARN_STREAM_NAMED("throttling_camera", "Simulation slowed down significantly; consider running"
+					                      "the simulation with a lower PX4_SIM_SPEED_FACTOR value (slowed down from "
+					                      << currentRate << " to " << nextRate << " updates per second)");
+				}
+				// We're discarding old samples to avoid extensive slowdown
+				timeSamples->reset();
+			}
+			PutCameraData(image, sensorUpdateTime);
+			PublishCameraInfo(sensorUpdateTime);
+		}
+		last_update_time_ = sensorUpdateTime;
+	}
+};
+}
+
+GZ_REGISTER_SENSOR_PLUGIN(throttling_camera::ThrottlingCamera);