Fix #435 (#438)

* Split Gazebo LED plugin to LED visual plugin and LED model plugin

* Get rid of unneeded 'Failed to convert' warning

* Minor

clover_simulation/src/sim_leds_controller.cpp

@@ -0,0 +1,71 @@
+#include <led_msgs/SetLEDs.h>
+#include <led_msgs/LEDStateArray.h>
+
+#include <ros/ros.h>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+
+class LedControllerPlugin : public gazebo::ModelPlugin {
+private:
+	std::unique_ptr<ros::NodeHandle> nh;
+	std::string ns;
+	ros::ServiceServer setLedsSrv;
+	led_msgs::LEDStateArray ledState;
+	ros::Publisher statePublisher;
+	std::mutex handleMutex;
+
+public:
+	bool setLeds(led_msgs::SetLEDs::Request &req, led_msgs::SetLEDs::Response &resp)
+	{
+		std::lock_guard<std::mutex> lock(handleMutex);
+		for(const auto& led : req.leds)
+		{
+			if (led.index < ledState.leds.size()) {
+				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;
+	}
+
+	virtual void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override
+	{
+		ROS_INFO("Initialize LED Controller");
+
+		// 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.");
+		}
+
+		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");
+		ledState.leds.resize(totalLeds);
+		for (int i = 0; i < totalLeds; i++) {
+			ledState.leds[i].index = i;
+		}
+
+		nh.reset(new ros::NodeHandle(ns));
+
+		setLedsSrv = nh->advertiseService("led/set_leds", &LedControllerPlugin::setLeds, this);
+		statePublisher = nh->advertise<led_msgs::LEDStateArray>("led/state", 1, true);
+
+		statePublisher.publish(ledState);
+	}
+};
+
+GZ_REGISTER_MODEL_PLUGIN(LedControllerPlugin);