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_description/urdf/leds/led_strip.xacro

@@ -64,6 +64,12 @@
         <!-- <gazebo>
             <static>true</static>
         </gazebo> -->
+        <gazebo>
+            <plugin name="${name}_ros_controller" filename="libsim_leds_controller.so">
+                <robotNamespace></robotNamespace>
+                <ledCount>${led_count}</ledCount>
+            </plugin>
+        </gazebo>
     </xacro:macro>
 
 </robot>

clover_simulation/CMakeLists.txt

@@ -37,6 +37,14 @@ target_compile_options(sim_leds PRIVATE -std=c++11)
 
 add_dependencies(sim_leds ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+add_library(sim_leds_controller src/sim_leds_controller.cpp)
+
+target_include_directories(sim_leds_controller PRIVATE ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
+target_link_libraries(sim_leds_controller PRIVATE ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+target_compile_options(sim_leds_controller PRIVATE -std=c++11)
+
+add_dependencies(sim_leds_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
 # Gazebo throttling camera plugin
 # for some reason, CMake does not support per-target link directories, and Gazebo does not put
 # CameraPlugin into ${GAZEBO_LIBRARIES}

clover_simulation/src/sim_leds.cpp

@@ -49,14 +49,9 @@ private:
 
 	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:
@@ -74,8 +69,6 @@ public:
 
 		nh.reset(new ros::NodeHandle(robotNamespace));
 
-		setLedsSrv = nh->advertiseService("led/set_leds", &LedController::setLeds, this); // the newer service server would overwrite the previous one
-		statePublisher = nh->advertise<led_msgs::LEDStateArray>("led/state", 1, true);
 		stateSubscriber = nh->subscribe<led_msgs::LEDStateArray>("led/state", 1, &LedController::handleLedsMsg, this);
 	};
 
@@ -90,12 +83,9 @@ public:
 		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;
-		statePublisher.publish(ledState);
 	}
 
 	void unregisterPlugin(sim_led::LedVisualPlugin* plugin)
@@ -150,7 +140,8 @@ public:
 		{
 			auto indexStr = parentName.substr(lastDashPos + 1);
 			try {
-				myIndex = std::stoi(indexStr);
+				if (indexStr == "visual") myIndex = 0; // the first visual doesn't have index
+				else myIndex = std::stoi(indexStr);
 			} catch(const std::exception &e) {
 				gzwarn << "Failed to convert " << indexStr << " to integer: " << e.what() <<  ", assuming 0\n";
 				myIndex = 0;
@@ -188,26 +179,6 @@ public:
 };
 }
 
-// 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);

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);