Class LedController

clover_simulation/src/sim_leds.cpp:34–102

LedController: a class that provides ROS interface for the LEDs.

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

	// LED state will be read from the topic to avoid creating more services
	ros::Subscriber stateSubscriber;

	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) in namespace '%s'",
			role == Role::Client ? "client" : "server", robotNamespace.c_str());

		nh.reset(new ros::NodeHandle(robotNamespace));

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