diff --git a/cob_light/common/include/distApproxMode.h b/cob_light/common/include/distApproxMode.h
index c3fb59196..d077f1332 100644
--- a/cob_light/common/include/distApproxMode.h
+++ b/cob_light/common/include/distApproxMode.h
@@ -67,11 +67,11 @@ class DistApproxMode : public Mode
             sectors.assign(_num_leds, 0.0);
 
             //calculate sector values
-            for(int i = 0; i < _num_leds; i++)
+            for(unsigned int i = 0; i < _num_leds; i++)
             {
                 float sum = std::numeric_limits<float>::max();
                 //float sum = 0;
-                for(int j = i*(int)sector_size; j < (i+1)*(int)sector_size; j++)
+                for(unsigned int j = i*(int)sector_size; j < (i+1)*(int)sector_size; j++)
                 {
                     if(ranges.at(j) != 0){
                         sum = std::min(ranges.at(j), sum);
@@ -80,7 +80,7 @@ class DistApproxMode : public Mode
                 sectors.at(i) = sum;
             }
 
-            for(int i = 0; i < _num_leds; i++)
+            for(unsigned int i = 0; i < _num_leds; i++)
             {
                 color::rgba col;
 
diff --git a/cob_light/common/include/sequenceMode.h b/cob_light/common/include/sequenceMode.h
index 987ff53bf..582c333fe 100644
--- a/cob_light/common/include/sequenceMode.h
+++ b/cob_light/common/include/sequenceMode.h
@@ -100,7 +100,7 @@ class SequenceMode : public Mode
 
 private:
   std::vector<seq_t> _seqences;
-  int _seqidx;
+  unsigned int _seqidx;
   bool _init;
   float _int_inc;
   float _int_count;
diff --git a/cob_light/common/include/serialIO.h b/cob_light/common/include/serialIO.h
index 83d78167c..7f6ef9fe1 100644
--- a/cob_light/common/include/serialIO.h
+++ b/cob_light/common/include/serialIO.h
@@ -71,6 +71,8 @@ class SerialIO
 	void start();
 	void stop();
 
+	const std::string& getDeviceString() const;
+
 private:
 	//ioQueue
 	ConcurrentQueue<std::vector<struct ioData> > _oQueue;
diff --git a/cob_light/common/include/signalLeftMode.h b/cob_light/common/include/signalLeftMode.h
new file mode 100644
index 000000000..a8f8fc7cb
--- /dev/null
+++ b/cob_light/common/include/signalLeftMode.h
@@ -0,0 +1,70 @@
+#ifndef SIGNAL_LEFT_H
+#define SIGNAL_LEFT_H
+
+#include <vector>
+
+#include "mode.h"
+
+class SignalLeft : public Mode
+{
+public:
+  SignalLeft(color::rgba color, uint16_t nLeds, uint8_t priority = 0, double freq = 0.25, uint8_t pulses = 0,
+             double timeout = 0)
+    : Mode(priority, freq, pulses, timeout), _num_leds(nLeds), _toggle(false), _timer_inc(0), leds_margin(5), leds_margin_left(7)
+  {
+    _color = color;
+    if (_pulses != 0)
+    {
+      _pulses *= 2;
+      _pulses += 1;
+    }
+    _inc = (1. / UPDATE_RATE_HZ) * _freq;
+  }
+
+  virtual void execute()
+  {
+    std::vector<color::rgba> cols;
+    color::rgba turnedOff;
+    turnedOff.a = 0;
+
+    for (uint16_t i = 0; i < (_num_leds / 2) + leds_margin; ++i)
+    {
+      cols.push_back(turnedOff);
+    }
+    for (uint16_t i = (_num_leds / 2) + leds_margin; i < _num_leds - leds_margin_left; ++i)
+    {
+      cols.push_back(_color);
+    }
+
+    if (_timer_inc >= 1.0)
+    {
+      for(color::rgba& color : cols)
+      {
+        color.a = color.a * (int)_toggle;
+      }
+      _pulsed++;
+      _toggle = !_toggle;
+      m_sigColorsReady(cols);
+      _timer_inc = 0.0;
+    }
+    else
+    {
+      _timer_inc += _inc;
+    }
+  }
+
+  std::string getName()
+  {
+    return std::string("SignalLeft");
+  }
+
+private:
+  uint16_t _num_leds;
+  bool _toggle;
+  double _timer_inc;
+  double _inc;
+  uint8_t leds_margin;
+  uint8_t leds_margin_left;
+};
+
+#endif  // !SIGNAL_LEFT_H
diff --git a/cob_light/common/include/signalRightMode.h b/cob_light/common/include/signalRightMode.h
new file mode 100644
index 000000000..6cd920212
--- /dev/null
+++ b/cob_light/common/include/signalRightMode.h
@@ -0,0 +1,75 @@
+#ifndef SIGNAL_RIGHT_H
+#define SIGNAL_RIGHT_H
+
+#include <vector>
+
+#include "mode.h"
+
+class SignalRight : public Mode
+{
+public:
+  SignalRight(color::rgba color, uint16_t nLeds, uint8_t priority = 0, double freq = 0.25, uint8_t pulses = 0,
+              double timeout = 0)
+    : Mode(priority, freq, pulses, timeout), _num_leds(nLeds), _toggle(false), _timer_inc(0), leds_margin(5)
+  {
+    _color = color;
+    if (_pulses != 0)
+    {
+      _pulses *= 2;
+      _pulses += 1;
+    }
+    _inc = (1. / UPDATE_RATE_HZ) * _freq;
+  }
+
+  virtual void execute()
+  {
+    std::vector<color::rgba> cols;
+    color::rgba turnedOff;
+    turnedOff.a = 0;
+
+    cols.push_back(turnedOff);
+    cols.push_back(turnedOff);
+    cols.push_back(turnedOff);
+    cols.push_back(turnedOff);
+    cols.push_back(turnedOff);
+
+    for (uint16_t i = leds_margin; i < (_num_leds / 2) - leds_margin; ++i)
+    {
+      cols.push_back(_color);
+    }
+    for (uint16_t i = (_num_leds / 2) - leds_margin; i < _num_leds; ++i)
+    {
+      cols.push_back(turnedOff);
+    }
+
+    if (_timer_inc >= 1.0)
+    {
+      for (color::rgba& color : cols)
+      {
+        color.a = color.a * (int)_toggle;
+      }
+      _pulsed++;
+      _toggle = !_toggle;
+      m_sigColorsReady(cols);
+      _timer_inc = 0.0;
+    }
+    else
+    {
+      _timer_inc += _inc;
+    }
+  }
+
+  std::string getName()
+  {
+    return std::string("SignalRIght");
+  }
+
+private:
+  uint16_t _num_leds;
+  bool _toggle;
+  double _timer_inc;
+  double _inc;
+  uint8_t leds_margin;
+};
+
+#endif  // !SIGNAL_RIGHT_H
diff --git a/cob_light/common/include/sweepColorMode.h b/cob_light/common/include/sweepColorMode.h
index be67f58b4..cccf10d3b 100644
--- a/cob_light/common/include/sweepColorMode.h
+++ b/cob_light/common/include/sweepColorMode.h
@@ -52,7 +52,7 @@ class SweepColorMode : public Mode
   {
     if(_timer_inc >= 1.0)
     {
-      for(int i = _num_leds/2; i < _num_leds; i++)
+      for(unsigned int i = _num_leds/2; i < _num_leds; i++)
       {
         _colors[i].a *= 0.7;
       }
@@ -82,7 +82,7 @@ class SweepColorMode : public Mode
   double _timer_inc;
   double _inc;
   size_t _num_leds;
-  int _pos;
+  unsigned int _pos;
   color::rgba _startcolor;
 };
 
diff --git a/cob_light/common/include/xmasMode.h b/cob_light/common/include/xmasMode.h
index 90b088983..49748acda 100644
--- a/cob_light/common/include/xmasMode.h
+++ b/cob_light/common/include/xmasMode.h
@@ -31,10 +31,10 @@ class XMasMode : public Mode
     if(_num_leds >= _chucksize*2)
     {
       int n_chunks = _num_leds / _chucksize + (_num_leds%_chucksize ? 1 : 0);
-      int k = 0;
+      unsigned int k = 0;
       for(int i = 0; i < n_chunks; i++)
       {
-        for(int j = 0; j < _chucksize; j++)
+        for(unsigned int j = 0; j < _chucksize; j++)
         {
           if(k >= _num_leds)
             break;
@@ -52,7 +52,7 @@ class XMasMode : public Mode
     }
     else
     {
-      for(int i = 0; i < _num_leds; i++)
+      for(unsigned int i = 0; i < _num_leds; i++)
       {
         if(i%2==0)
         {
diff --git a/cob_light/common/src/modeFactory.cpp b/cob_light/common/src/modeFactory.cpp
index f94789f6f..a1b37396d 100644
--- a/cob_light/common/src/modeFactory.cpp
+++ b/cob_light/common/src/modeFactory.cpp
@@ -14,18 +14,19 @@
  * limitations under the License.
  */
 
-
-#include <modeFactory.h>
-#include <staticMode.h>
-#include <flashMode.h>
-#include <breathMode.h>
 #include <breathColorMode.h>
-#include <fadeColorMode.h>
-#include <sequenceMode.h>
+#include <breathMode.h>
 #include <circleColorMode.h>
-#include <sweepColorMode.h>
 #include <distApproxMode.h>
+#include <fadeColorMode.h>
+#include <flashMode.h>
 #include <glowColorMode.h>
+#include <modeFactory.h>
+#include <sequenceMode.h>
+#include <signalLeftMode.h>
+#include <signalRightMode.h>
+#include <staticMode.h>
+#include <sweepColorMode.h>
 #include <xmasMode.h>
 
 ModeFactory::ModeFactory()
@@ -37,189 +38,199 @@ ModeFactory::~ModeFactory()
 
 boost::shared_ptr<Mode> ModeFactory::create(cob_light::LightMode requestMode, IColorO* colorO)
 {
-    boost::shared_ptr<Mode> mode;
-    color::rgba color;
-    color.r = requestMode.colors[0].r;
-    color.g = requestMode.colors[0].g;
-    color.b = requestMode.colors[0].b;
-    color.a = requestMode.colors[0].a;
-
-    switch(requestMode.mode)
-    {
+  boost::shared_ptr<Mode> mode;
+  color::rgba color;
+  color.r = requestMode.colors[0].r;
+  color.g = requestMode.colors[0].g;
+  color.b = requestMode.colors[0].b;
+  color.a = requestMode.colors[0].a;
+
+  switch (requestMode.mode)
+  {
     case cob_light::LightModes::STATIC:
-        mode.reset(new StaticMode(color, requestMode.priority, requestMode.frequency,\
-                                  requestMode.pulses, requestMode.timeout));
-        break;
+      mode.reset(
+          new StaticMode(color, requestMode.priority, requestMode.frequency, requestMode.pulses, requestMode.timeout));
+      break;
 
     case cob_light::LightModes::FLASH:
-        mode.reset(new FlashMode(color, requestMode.priority, requestMode.frequency,\
-                                 requestMode.pulses, requestMode.timeout));
-        break;
+      mode.reset(
+          new FlashMode(color, requestMode.priority, requestMode.frequency, requestMode.pulses, requestMode.timeout));
+      break;
 
     case cob_light::LightModes::BREATH:
-        mode.reset(new BreathMode(color, requestMode.priority, requestMode.frequency,\
-                                  requestMode.pulses, requestMode.timeout));
-        break;
+      mode.reset(
+          new BreathMode(color, requestMode.priority, requestMode.frequency, requestMode.pulses, requestMode.timeout));
+      break;
 
     case cob_light::LightModes::BREATH_COLOR:
-        mode.reset(new BreathColorMode(color, requestMode.priority, requestMode.frequency,\
-                                       requestMode.pulses, requestMode.timeout));
-        break;
+      mode.reset(new BreathColorMode(color, requestMode.priority, requestMode.frequency, requestMode.pulses,
+                                     requestMode.timeout));
+      break;
 
     case cob_light::LightModes::FADE_COLOR:
-        mode.reset(new FadeColorMode(color, requestMode.priority, requestMode.frequency,\
-                                     requestMode.pulses, requestMode.timeout));
-        break;
+      mode.reset(new FadeColorMode(color, requestMode.priority, requestMode.frequency, requestMode.pulses,
+                                   requestMode.timeout));
+      break;
+
+    case cob_light::LightModes::SIGNAL_LEFT:
+      mode.reset(new SignalLeft(color, colorO->getNumLeds(), requestMode.priority, requestMode.frequency,
+                                requestMode.pulses, requestMode.timeout));
+      break;
+
+    case cob_light::LightModes::SIGNAL_RIGHT:
+      mode.reset(new SignalRight(color, colorO->getNumLeds(), requestMode.priority, requestMode.frequency,
+                                requestMode.pulses, requestMode.timeout));
+      break;
 
     case cob_light::LightModes::SEQ:
     {
-        std::vector<seq_t> seqs;
-        for(size_t i = 0; i < requestMode.sequences.size(); i++)
-        {
-            seq_t seq;
-            seq.color.r = requestMode.sequences[i].color.r;
-            seq.color.g = requestMode.sequences[i].color.g;
-            seq.color.b = requestMode.sequences[i].color.b;
-            seq.color.a = requestMode.sequences[i].color.a;
-            seq.holdtime = requestMode.sequences[i].hold_time;
-            seq.crosstime = requestMode.sequences[i].cross_time;
-            seqs.push_back(seq);
-            std::cout<<"got new seq: "<<seq.color.r<<" "<<seq.color.g<<" "<<seq.color.b<<std::endl;
-        }
-        mode.reset(new SequenceMode(seqs, requestMode.priority, requestMode.frequency,\
-                                    requestMode.pulses, requestMode.timeout));
+      std::vector<seq_t> seqs;
+      for (size_t i = 0; i < requestMode.sequences.size(); i++)
+      {
+        seq_t seq;
+        seq.color.r = requestMode.sequences[i].color.r;
+        seq.color.g = requestMode.sequences[i].color.g;
+        seq.color.b = requestMode.sequences[i].color.b;
+        seq.color.a = requestMode.sequences[i].color.a;
+        seq.holdtime = requestMode.sequences[i].hold_time;
+        seq.crosstime = requestMode.sequences[i].cross_time;
+        seqs.push_back(seq);
+        std::cout << "got new seq: " << seq.color.r << " " << seq.color.g << " " << seq.color.b << std::endl;
+      }
+      mode.reset(
+          new SequenceMode(seqs, requestMode.priority, requestMode.frequency, requestMode.pulses, requestMode.timeout));
     }
-        break;
+    break;
 
     case cob_light::LightModes::CIRCLE_COLORS:
     {
-        std::vector<color::rgba> colors;
-        if(requestMode.colors.empty())
+      std::vector<color::rgba> colors;
+      if (requestMode.colors.empty())
+      {
+        colors.push_back(color);
+      }
+      else
+      {
+        for (size_t i = 0; i < requestMode.colors.size(); i++)
         {
-            colors.push_back(color);
+          color.r = requestMode.colors[i].r;
+          color.g = requestMode.colors[i].g;
+          color.b = requestMode.colors[i].b;
+          color.a = requestMode.colors[i].a;
+          colors.push_back(color);
         }
-        else
-        {
-            for(size_t i = 0; i < requestMode.colors.size(); i++)
-            {
-                color.r = requestMode.colors[i].r;
-                color.g = requestMode.colors[i].g;
-                color.b = requestMode.colors[i].b;
-                color.a = requestMode.colors[i].a;
-                colors.push_back(color);
-            }
-        }
-        mode.reset(new CircleColorMode(colors, colorO->getNumLeds(), requestMode.priority, requestMode.frequency,\
-                                       requestMode.pulses, requestMode.timeout));
+      }
+      mode.reset(new CircleColorMode(colors, colorO->getNumLeds(), requestMode.priority, requestMode.frequency,
+                                     requestMode.pulses, requestMode.timeout));
     }
-        break;
+    break;
 
     case cob_light::LightModes::SWEEP:
     {
-        std::vector<color::rgba> colors;
-        if(requestMode.colors.empty())
+      std::vector<color::rgba> colors;
+      if (requestMode.colors.empty())
+      {
+        colors.push_back(color);
+      }
+      else
+      {
+        for (size_t i = 0; i < requestMode.colors.size(); i++)
         {
-            colors.push_back(color);
+          color.r = requestMode.colors[i].r;
+          color.g = requestMode.colors[i].g;
+          color.b = requestMode.colors[i].b;
+          color.a = requestMode.colors[i].a;
+          colors.push_back(color);
         }
-        else
-        {
-            for(size_t i = 0; i < requestMode.colors.size(); i++)
-            {
-                color.r = requestMode.colors[i].r;
-                color.g = requestMode.colors[i].g;
-                color.b = requestMode.colors[i].b;
-                color.a = requestMode.colors[i].a;
-                colors.push_back(color);
-            }
-        }
-        mode.reset(new SweepColorMode(colors, colorO->getNumLeds(), requestMode.priority, requestMode.frequency,
-                                      requestMode.pulses, requestMode.timeout));
+      }
+      mode.reset(new SweepColorMode(colors, colorO->getNumLeds(), requestMode.priority, requestMode.frequency,
+                                    requestMode.pulses, requestMode.timeout));
     }
-        break;
+    break;
 
     case cob_light::LightModes::DIST_APPROX:
-        mode.reset(new DistApproxMode(colorO->getNumLeds(), requestMode.priority, requestMode.frequency,
-                                      requestMode.pulses, requestMode.timeout));
-        break;
+      mode.reset(new DistApproxMode(colorO->getNumLeds(), requestMode.priority, requestMode.frequency,
+                                    requestMode.pulses, requestMode.timeout));
+      break;
 
     case cob_light::LightModes::GLOW:
-        mode.reset(new GlowColorMode(color, requestMode.priority, requestMode.frequency,\
-                                     requestMode.pulses, requestMode.timeout));
-        break;
+      mode.reset(new GlowColorMode(color, requestMode.priority, requestMode.frequency, requestMode.pulses,
+                                   requestMode.timeout));
+      break;
 
     case cob_light::LightModes::XMAS:
-        mode.reset(new XMasMode(colorO->getNumLeds(), requestMode.priority, requestMode.frequency,\
-                                     requestMode.pulses, requestMode.timeout));
-        break;
+      mode.reset(new XMasMode(colorO->getNumLeds(), requestMode.priority, requestMode.frequency, requestMode.pulses,
+                              requestMode.timeout));
+      break;
 
     default:
-        mode.reset();
-    }
+      mode.reset();
+  }
 
-    return mode;
+  return mode;
 }
 
 boost::shared_ptr<Mode> ModeFactory::create(std::string requestMode, color::rgba color)
 {
-    boost::shared_ptr<Mode> mode;
-
-    if(requestMode == "Static" || requestMode == "static" || requestMode == "STATIC")
-    {
-        mode.reset(new StaticMode(color));
-    }
-    else if(requestMode == "Flash" || requestMode == "flash" || requestMode == "FLASH")
-    {
-        mode.reset(new FlashMode(color));
-    }
-    else if(requestMode == "Breath" || requestMode == "breath" || requestMode == "BREATH")
-    {
-        mode.reset(new BreathMode(color));
-    }
-    else if(requestMode == "BreathColor" || requestMode == "BreathColor" || requestMode == "BreathColor" ||
-            requestMode == "Breath_Color" || requestMode == "breath_color" || requestMode == "BREATH_COLOR")
-    {
-        mode.reset(new BreathColorMode(color));
-    }
-    else if(requestMode == "FadeColor" || requestMode == "fadecolor" || requestMode == "FADECOLOR" ||
-            requestMode == "Fade_Color" || requestMode == "fade_color" || requestMode == "FADE_COLOR")
-    {
-        mode.reset(new FadeColorMode(color));
-    }
-    else
-        mode.reset();
-
-    return mode;
+  boost::shared_ptr<Mode> mode;
+
+  if (requestMode == "Static" || requestMode == "static" || requestMode == "STATIC")
+  {
+    mode.reset(new StaticMode(color));
+  }
+  else if (requestMode == "Flash" || requestMode == "flash" || requestMode == "FLASH")
+  {
+    mode.reset(new FlashMode(color));
+  }
+  else if (requestMode == "Breath" || requestMode == "breath" || requestMode == "BREATH")
+  {
+    mode.reset(new BreathMode(color));
+  }
+  else if (requestMode == "BreathColor" || requestMode == "BreathColor" || requestMode == "BreathColor" ||
+           requestMode == "Breath_Color" || requestMode == "breath_color" || requestMode == "BREATH_COLOR")
+  {
+    mode.reset(new BreathColorMode(color));
+  }
+  else if (requestMode == "FadeColor" || requestMode == "fadecolor" || requestMode == "FADECOLOR" ||
+           requestMode == "Fade_Color" || requestMode == "fade_color" || requestMode == "FADE_COLOR")
+  {
+    mode.reset(new FadeColorMode(color));
+  }
+  else
+    mode.reset();
+
+  return mode;
 }
 
-int ModeFactory::type(Mode *mode)
+int ModeFactory::type(Mode* mode)
 {
-    int ret;
-    if (mode == NULL)
-        ret = cob_light::LightModes::NONE;
-    else if(dynamic_cast<StaticMode*>(mode) != NULL)
-        ret = cob_light::LightModes::STATIC;
-    else if(dynamic_cast<FlashMode*>(mode) != NULL)
-        ret = cob_light::LightModes::FLASH;
-    else if(dynamic_cast<BreathMode*>(mode) != NULL)
-        ret = cob_light::LightModes::BREATH;
-    else if(dynamic_cast<BreathColorMode*>(mode) != NULL)
-        ret = cob_light::LightModes::BREATH_COLOR;
-    else if(dynamic_cast<FadeColorMode*>(mode) != NULL)
-        ret = cob_light::LightModes::FADE_COLOR;
-    else if(dynamic_cast<SequenceMode*>(mode) != NULL)
-        ret = cob_light::LightModes::SEQ;
-    else if(dynamic_cast<CircleColorMode*>(mode) != NULL)
-        ret = cob_light::LightModes::CIRCLE_COLORS;
-    else if(dynamic_cast<SweepColorMode*>(mode) != NULL)
-        ret = cob_light::LightModes::SWEEP;
-    else if(dynamic_cast<DistApproxMode*>(mode) != NULL)
-        ret = cob_light::LightModes::DIST_APPROX;
-    else if(dynamic_cast<GlowColorMode*>(mode) != NULL)
-        ret = cob_light::LightModes::GLOW;
-    else if(dynamic_cast<XMasMode*>(mode) != NULL)
-        ret = cob_light::LightModes::XMAS;
-    else
-        ret = cob_light::LightModes::NONE;
-
-    return ret;
+  int ret;
+  if (mode == NULL)
+    ret = cob_light::LightModes::NONE;
+  else if (dynamic_cast<StaticMode*>(mode) != NULL)
+    ret = cob_light::LightModes::STATIC;
+  else if (dynamic_cast<FlashMode*>(mode) != NULL)
+    ret = cob_light::LightModes::FLASH;
+  else if (dynamic_cast<BreathMode*>(mode) != NULL)
+    ret = cob_light::LightModes::BREATH;
+  else if (dynamic_cast<BreathColorMode*>(mode) != NULL)
+    ret = cob_light::LightModes::BREATH_COLOR;
+  else if (dynamic_cast<FadeColorMode*>(mode) != NULL)
+    ret = cob_light::LightModes::FADE_COLOR;
+  else if (dynamic_cast<SequenceMode*>(mode) != NULL)
+    ret = cob_light::LightModes::SEQ;
+  else if (dynamic_cast<CircleColorMode*>(mode) != NULL)
+    ret = cob_light::LightModes::CIRCLE_COLORS;
+  else if (dynamic_cast<SweepColorMode*>(mode) != NULL)
+    ret = cob_light::LightModes::SWEEP;
+  else if (dynamic_cast<DistApproxMode*>(mode) != NULL)
+    ret = cob_light::LightModes::DIST_APPROX;
+  else if (dynamic_cast<GlowColorMode*>(mode) != NULL)
+    ret = cob_light::LightModes::GLOW;
+  else if (dynamic_cast<XMasMode*>(mode) != NULL)
+    ret = cob_light::LightModes::XMAS;
+  else
+    ret = cob_light::LightModes::NONE;
+
+  return ret;
 }
diff --git a/cob_light/common/src/serialIO.cpp b/cob_light/common/src/serialIO.cpp
index 4fc0ddd84..3414f5f51 100644
--- a/cob_light/common/src/serialIO.cpp
+++ b/cob_light/common/src/serialIO.cpp
@@ -33,6 +33,11 @@ SerialIO::~SerialIO()
 	closePort();
 }
 
+const std::string& SerialIO::getDeviceString() const
+{
+	return _device_string;
+}
+
 // Open Serial Port
 int SerialIO::openPort(std::string devicestring, int baudrate)
 {
diff --git a/cob_light/msg/LightModes.msg b/cob_light/msg/LightModes.msg
index 6319e4eda..d7d1ecf0c 100644
--- a/cob_light/msg/LightModes.msg
+++ b/cob_light/msg/LightModes.msg
@@ -12,3 +12,5 @@ uint8 SWEEP =          8           # circle color from front to back on both sid
 uint8 DIST_APPROX =    9           # color led circle from green to red depended on the measured laserscan dists
 uint8 GLOW =          10           # glow the color
 uint8 XMAS =          11           # red white christmas mode
+uint8 SIGNAL_LEFT =   12           # Turn signal to the left
+uint8 SIGNAL_RIGHT =  13           # Turn signal to the right
diff --git a/cob_light/ros/include/colorOSim.h b/cob_light/ros/include/colorOSim.h
index b165b7fe8..c4a099b15 100644
--- a/cob_light/ros/include/colorOSim.h
+++ b/cob_light/ros/include/colorOSim.h
@@ -14,7 +14,6 @@
  * limitations under the License.
  */
 
-
 #ifndef COLOROSIM_H
 #define COLOROSIM_H
 
@@ -24,15 +23,18 @@
 #include <colorUtils.h>
 #include <ros/ros.h>
 
+bool equals(const float lhs, const float rhs, const float a_precision);
+
 class ColorOSim : public IColorO
 {
 public:
+  ColorOSim() = delete;
   ColorOSim(ros::NodeHandle* nh);
-  virtual ~ColorOSim();
+  virtual ~ColorOSim() = default;
 
   bool init();
   void setColor(color::rgba color);
-  void setColorMulti(std::vector<color::rgba> &colors);
+  void setColorMulti(std::vector<color::rgba>& colors);
 
 private:
   ros::NodeHandle* p_nh;
diff --git a/cob_light/ros/src/cob_light.cpp b/cob_light/ros/src/cob_light.cpp
index 4e09e2fc6..887c35b69 100644
--- a/cob_light/ros/src/cob_light.cpp
+++ b/cob_light/ros/src/cob_light.cpp
@@ -14,7 +14,6 @@
  * limitations under the License.
  */
 
-
 // standard includes
 #include <stdio.h>
 #include <string.h>
@@ -54,7 +53,7 @@
 
 sig_atomic_t volatile gShutdownRequest = 0;
 
-void sigIntHandler(int signal)
+void sigIntHandler(int /*signal*/)
 {
   ::gShutdownRequest = 1;
 }
@@ -67,8 +66,8 @@ void shutdownCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
   if (num_params > 1)
   {
     std::string reason = params[1];
-    ROS_WARN("Shutdown request received. Reason: [%s]", reason.c_str());
-    ::gShutdownRequest = 1; // Set flag
+    ROS_WARN("shutdown request received. Reason: [%s]", reason.c_str());
+    ::gShutdownRequest = 1;  // Set flag
   }
 
   result = ros::xmlrpc::responseInt(1, "", 0);
@@ -77,8 +76,7 @@ void shutdownCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
 class LightControl
 {
 public:
-  LightControl() :
-    _invertMask(0), _topic_priority(0)
+  LightControl() : _invertMask(0), _topic_priority(0)
   {
   }
   bool init()
@@ -89,48 +87,51 @@ class LightControl
     std::string startup_mode;
     p_colorO = NULL;
     p_modeExecutor = NULL;
-    //diagnostics
+    // diagnostics
     _pubDiagnostic = _nh.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
     _diagnostics_timer = _nh.createTimer(ros::Duration(1.0), &LightControl::publish_diagnostics_cb, this);
 
     diagnostic_msgs::DiagnosticStatus status;
     status.name = ros::this_node::getName();
 
-    //Get Parameter from Parameter Server
+    // Get Parameter from Parameter Server
     _nh = ros::NodeHandle("~");
-    if(!_nh.hasParam("invert_output"))
+    if (!_nh.hasParam("invert_output"))
       ROS_WARN("Parameter 'invert_output' is missing. Using default Value: false");
     _nh.param<bool>("invert_output", invert_output, false);
     _invertMask = (int)invert_output;
 
-    if(!_nh.hasParam("devicedriver"))
+    if (!_nh.hasParam("devicedriver"))
       ROS_WARN("Parameter 'devicedriver' is missing. Using default Value: cob_ledboard");
-    _nh.param<std::string>("devicedriver",_deviceDriver,"cob_ledboard");
+    _nh.param<std::string>("devicedriver", _deviceDriver, "cob_ledboard");
 
-    if(!_nh.hasParam("devicestring"))
+    if (!_nh.hasParam("devicestring"))
       ROS_WARN("Parameter 'devicestring' is missing. Using default Value: /dev/ttyLed");
-    _nh.param<std::string>("devicestring",_deviceString,"/dev/ttyLed");
+    _nh.param<std::string>("devicestring", _deviceString, "/dev/ttyLed");
 
-    if(!_nh.hasParam("baudrate"))
+    if (!_nh.hasParam("baudrate"))
       ROS_WARN("Parameter 'baudrate' is missing. Using default Value: 230400");
-    _nh.param<int>("baudrate",_baudrate,230400);
+    _nh.param<int>("baudrate", _baudrate, 230400);
 
-    if(!_nh.hasParam("pubmarker"))
+    if (!_nh.hasParam("pubmarker"))
       ROS_WARN("Parameter 'pubmarker' is missing. Using default Value: true");
-    _nh.param<bool>("pubmarker",_bPubMarker,true);
+    _nh.param<bool>("pubmarker", _bPubMarker, true);
 
-    if(!_nh.hasParam("marker_frame"))
+    if (!_nh.hasParam("marker_frame"))
       ROS_WARN("Parameter 'marker_frame' is missing. Using default Value: /base_link");
-    _nh.param<std::string>("marker_frame",_sMarkerFrame,"base_link");
+    _nh.param<std::string>("marker_frame", _sMarkerFrame, "base_link");
 
-    if(!_nh.hasParam("sim_enabled"))
+    if (!_nh.hasParam("sim_enabled"))
       ROS_WARN("Parameter 'sim_enabled' is missing. Using default Value: false");
     _nh.param<bool>("sim_enabled", _bSimEnabled, false);
 
-    if(!_nh.hasParam("startup_color"))
+    if (!_nh.hasParam("startup_color"))
     {
       ROS_WARN("Parameter 'startup_color' is missing. Using default Value: off");
-      _color.r=0;_color.g=0;_color.b=0;_color.a=0;
+      _color.r = 0;
+      _color.g = 0;
+      _color.b = 0;
+      _color.a = 0;
     }
     else
     {
@@ -144,58 +145,58 @@ class LightControl
       _color.a = static_cast<double>(param_list[3]);
     }
 
-    if(!_nh.hasParam("startup_mode"))
+    if (!_nh.hasParam("startup_mode"))
       ROS_WARN("Parameter 'startup_mode' is missing. Using default Value: None");
     _nh.param<std::string>("startup_mode", startup_mode, "None");
 
-    if(!_nh.hasParam("num_leds"))
-	    ROS_WARN("Parameter 'num_leds' is missing. Using default Value: 58");
- 	  _nh.param<int>("num_leds", _num_leds, 58);
+    if (!_nh.hasParam("num_leds"))
+      ROS_WARN("Parameter 'num_leds' is missing. Using default Value: 58");
+    _nh.param<int>("num_leds", _num_leds, 58);
 
     int led_offset;
     _nh.param<int>("led_offset", led_offset, 0);
 
-    //Subscribe to LightController Command Topic
+    // Subscribe to LightController Command Topic
     _sub = _nh.subscribe("command", 1, &LightControl::topicCallback, this);
 
-    //Advertise light mode Service
+    // Advertise light mode Service
     _srvServer = _nh.advertiseService("set_light", &LightControl::serviceCallback, this);
 
-    //Advertise stop mode service
+    // Advertise stop mode service
     _srvStopMode = _nh.advertiseService("stop_mode", &LightControl::stopMode, this);
 
-    //Start light mode Action Server
+    // Start light mode Action Server
     _as = new ActionServer(_nh, "set_light", boost::bind(&LightControl::actionCallback, this, _1), false);
     _as->start();
 
-    //Advertise visualization marker topic
-    _pubMarker = _nh.advertise<visualization_msgs::Marker>("marker",1);
+    // Advertise visualization marker topic
+    _pubMarker = _nh.advertise<visualization_msgs::Marker>("marker", 1);
 
-    if(!_bSimEnabled)
+    if (!_bSimEnabled)
     {
-      //open serial port
-      ROS_INFO("Open Port on %s",_deviceString.c_str());
-      if(_serialIO.openPort(_deviceString, _baudrate) != -1)
+      // open serial port
+      ROS_INFO("Open Port on %s", _deviceString.c_str());
+      if (_serialIO.openPort(_deviceString, _baudrate) != -1)
       {
         ROS_INFO("Serial connection on %s succeeded.", _deviceString.c_str());
         status.level = 0;
         status.message = "light controller running";
 
-        if(_deviceDriver == "cob_ledboard")
+        if (_deviceDriver == "cob_ledboard")
           p_colorO = new ColorO(&_serialIO);
-        else if(_deviceDriver == "ms-35")
+        else if (_deviceDriver == "ms-35")
           p_colorO = new MS35(&_serialIO);
-        else if(_deviceDriver == "stageprofi")
+        else if (_deviceDriver == "stageprofi")
           p_colorO = new StageProfi(&_serialIO, _num_leds, led_offset);
         else
         {
-          ROS_ERROR_STREAM("Unsupported devicedriver ["<<_deviceDriver<<"], falling back to sim mode");
+          ROS_ERROR_STREAM("Unsupported devicedriver [" << _deviceDriver << "], falling back to sim mode");
           p_colorO = new ColorOSim(&_nh);
           status.level = 2;
           status.message = "Unsupported devicedriver. Running in simulation mode";
         }
         p_colorO->setMask(_invertMask);
-        if(!p_colorO->init())
+        if (!p_colorO->init())
         {
           status.level = 3;
           status.message = "Initializing connection to driver failed";
@@ -225,16 +226,16 @@ class LightControl
 
     _diagnostics.status.push_back(status);
 
-    if(!ret)
+    if (!ret)
       return false;
 
-    if(_bPubMarker)
+    if (_bPubMarker)
       p_colorO->signalColorSet()->connect(boost::bind(&LightControl::markerCallback, this, _1));
 
     p_modeExecutor = new ModeExecutor(p_colorO);
 
     boost::shared_ptr<Mode> mode = ModeFactory::create(startup_mode, _color);
-    if(mode == NULL)
+    if (mode == NULL)
     {
       p_colorO->setColor(_color);
     }
@@ -246,12 +247,12 @@ class LightControl
 
   ~LightControl()
   {
-    if(p_modeExecutor != NULL)
+    if (p_modeExecutor != NULL)
     {
       p_modeExecutor->stop();
       delete p_modeExecutor;
     }
-    if(p_colorO != NULL)
+    if (p_colorO != NULL)
     {
       delete p_colorO;
     }
@@ -259,138 +260,167 @@ class LightControl
 
   void topicCallback(cob_light::ColorRGBAArray color)
   {
-      boost::mutex::scoped_lock lock(_mutex);
-      if(p_modeExecutor->getExecutingPriority() <= _topic_priority)
+    boost::mutex::scoped_lock lock(_mutex);
+    if (p_modeExecutor->getExecutingPriority() <= _topic_priority)
+    {
+      p_modeExecutor->pause();
+      if (color.colors.size() > 0)
       {
-          p_modeExecutor->pause();
-          if(color.colors.size() > 0)
+        if (color.colors.size() > 1)
+        {
+          std::vector<color::rgba> colors;
+          for (size_t i = 0; i < colors.size(); i++)
           {
-              if(color.colors.size() > 1)
-              {
-                  std::vector<color::rgba> colors;
-                  for(size_t i=0; i<colors.size();i++)
-                  {
-                      if(color.colors[i].r <= 1.0 && color.colors[i].g <= 1.0 && color.colors[i].b <= 1.0)
-                      {
-                        _color.a = color.colors[i].a;
-                        _color.r = color.colors[i].r;
-                        _color.g = color.colors[i].g;
-                        _color.b = color.colors[i].b;
-                        colors.push_back(_color);
-                      }
-                      else
-                        ROS_ERROR("Unsupported Color format. rgba values range is between 0.0 - 1.0");
-                  }
-                  p_colorO->setColorMulti(colors);
-              }
-              else
-              {
-                  if(color.colors[0].r <= 1.0 && color.colors[0].g <= 1.0 && color.colors[0].b <= 1.0)
-                  {
-                      _color.r = color.colors[0].r;
-                      _color.g = color.colors[0].g;
-                      _color.b = color.colors[0].b;
-                      _color.a = color.colors[0].a;
-
-                      p_colorO->setColor(_color);
-                  }
-                  else
-                    ROS_ERROR("Unsupported Color format. rgba values range is between 0.0 - 1.0");
-              }
+            if (color.colors[i].r <= 1.0 && color.colors[i].g <= 1.0 && color.colors[i].b <= 1.0)
+            {
+              _color.a = color.colors[i].a;
+              _color.r = color.colors[i].r;
+              _color.g = color.colors[i].g;
+              _color.b = color.colors[i].b;
+              colors.push_back(_color);
+            }
+            else
+              ROS_ERROR("Unsupported Color format. rgba values range is between 0.0 - 1.0");
+          }
+          p_colorO->setColorMulti(colors);
+        }
+        else
+        {
+          if (color.colors[0].r <= 1.0 && color.colors[0].g <= 1.0 && color.colors[0].b <= 1.0)
+          {
+            _color.r = color.colors[0].r;
+            _color.g = color.colors[0].g;
+            _color.b = color.colors[0].b;
+            _color.a = color.colors[0].a;
+
+            p_colorO->setColor(_color);
           }
           else
-            ROS_ERROR("Empty color msg received");
+            ROS_ERROR("Unsupported Color format. rgba values range is between 0.0 - 1.0");
         }
+      }
+      else
+        ROS_ERROR("Empty color msg received");
+    }
   }
 
-  bool serviceCallback(cob_light::SetLightMode::Request &req, cob_light::SetLightMode::Response &res)
+  bool serviceCallback(cob_light::SetLightMode::Request& req, cob_light::SetLightMode::Response& res)
   {
     boost::mutex::scoped_lock lock(_mutex);
     bool ret = false;
 
-    //ROS_DEBUG("Service Callback [Mode: %i with prio: %i freq: %f timeout: %f pulses: %i ] [R: %f with G: %f B: %f A: %f]",
-    //	req.mode.mode, req.mode.priority, req.mode.frequency, req.mode.timeout, req.mode.pulses,req.mode.color.r,req.mode.color.g ,req.mode.color.b,req.mode.color.a);
-    if(req.mode.colors.size() > 0)
+    // ROS_DEBUG("Service Callback [Mode: %i with prio: %i freq: %f timeout: %f pulses: %i ] [R: %f with G: %f B: %f A:
+    // %f]", 	req.mode.mode, req.mode.priority, req.mode.frequency, req.mode.timeout,
+    //req.mode.pulses,req.mode.color.r,req.mode.color.g ,req.mode.color.b,req.mode.color.a);
+    if (req.mode.colors.size() > 0)
     {
-        if(req.mode.colors[0].r > 1.0 || req.mode.colors[0].g > 1.0 ||
-            req.mode.colors[0].b > 1.0 || req.mode.colors[0].a > 1.0)
-        {
-          res.active_mode = p_modeExecutor->getExecutingMode();
-          res.active_priority = p_modeExecutor->getExecutingPriority();
-          res.track_id = -1;
-          ROS_ERROR("Unsupported Color format. rgba values range is between 0.0 - 1.0");
-        }
-        else if(req.mode.mode == cob_light::LightModes::NONE) //refactor this
-        {
-          p_modeExecutor->stop();
-          _color.a = 0;
-          //p_modeExecutor->execute(req.mode);
-          p_colorO->setColor(_color);
-          res.active_mode = p_modeExecutor->getExecutingMode();
-          res.active_priority = p_modeExecutor->getExecutingPriority();
-          ret = true;
-        }
-        else
-        {
-          uint64_t u_id = p_modeExecutor->execute(req.mode);
-          res.active_mode = p_modeExecutor->getExecutingMode();
-          res.active_priority = p_modeExecutor->getExecutingPriority();
-          res.track_id = u_id;
-          ret = true;
-        }
+      if (req.mode.colors[0].r > 1.0 || req.mode.colors[0].g > 1.0 || req.mode.colors[0].b > 1.0 ||
+          req.mode.colors[0].a > 1.0)
+      {
+        res.active_mode = p_modeExecutor->getExecutingMode();
+        res.active_priority = p_modeExecutor->getExecutingPriority();
+        res.track_id = -1;
+        ROS_ERROR("Unsupported Color format. rgba values range is between 0.0 - 1.0");
+      }
+      else if (req.mode.mode == cob_light::LightModes::NONE)  // refactor this
+      {
+        p_modeExecutor->stop();
+        _color.a = 0;
+        // p_modeExecutor->execute(req.mode);
+        p_colorO->setColor(_color);
+        res.active_mode = p_modeExecutor->getExecutingMode();
+        res.active_priority = p_modeExecutor->getExecutingPriority();
+        ret = true;
+      }
+      else
+      {
+        uint64_t u_id = p_modeExecutor->execute(req.mode);
+        res.active_mode = p_modeExecutor->getExecutingMode();
+        res.active_priority = p_modeExecutor->getExecutingPriority();
+        res.track_id = u_id;
+        ret = true;
+      }
     }
     return ret;
   }
 
-  void actionCallback(const cob_light::SetLightModeGoalConstPtr &goal)
+  void print_leds(const cob_light::LightMode_<std::allocator<void>>::_colors_type& colors)
   {
+    std::string led_string;
+    for (size_t j = 0; j < colors.size(); ++j)
+    {
+      //if led is black
+      if (equals(colors[j].r, 0.0f, 0.01f) && equals(colors[j].g, 0.0f, 0.01f) &&
+          equals(colors[j].b, 0.0f, 0.01f))
+      {
+        led_string.append(".");
+      }
+      else
+      {
+        led_string.append("O");
+      }
+    }
+
+    ROS_INFO_STREAM("[cob_light] LEDS: " << led_string);
+  }
+
+  void actionCallback(const cob_light::SetLightModeGoalConstPtr& goal)
+  {
+    ROS_WARN("[cob_light actionCallback()] Received a LED command. ");
+
     boost::mutex::scoped_lock lock(_mutex);
     cob_light::SetLightModeResult result;
-    if(goal->mode.colors.size() > 0)
+    if (!goal->mode.colors.empty())
     {
-        if(goal->mode.colors[0].r > 1.0 || goal->mode.colors[0].g > 1.0 ||
-            goal->mode.colors[0].b > 1.0 || goal->mode.colors[0].a > 1.0)
-        {
-          result.active_mode = p_modeExecutor->getExecutingMode();
-          result.active_priority = p_modeExecutor->getExecutingPriority();
-          result.track_id = -1;
-          _as->setAborted(result, "Unsupported Color format. rgba values range is between 0.0 - 1.0");
+      if (goal->mode.colors[0].r > 1.0 || goal->mode.colors[0].g > 1.0 || goal->mode.colors[0].b > 1.0 ||
+          goal->mode.colors[0].a > 1.0)
+      {
+        result.active_mode = p_modeExecutor->getExecutingMode();
+        result.active_priority = p_modeExecutor->getExecutingPriority();
+        result.track_id = -1;
+        _as->setAborted(result, "Unsupported Color format. rgba values range is between 0.0 - 1.0");
 
-          ROS_ERROR("Unsupported Color format. rgba values range is between 0.0 - 1.0");
-        }
-        else if(goal->mode.mode == cob_light::LightModes::NONE)
-        {
-          p_modeExecutor->stop();
-          _color.a = 0;
-          p_colorO->setColor(_color);
-          result.active_mode = p_modeExecutor->getExecutingMode();
-          result.active_priority = p_modeExecutor->getExecutingPriority();
-          result.track_id = -1;
-          _as->setSucceeded(result, "Mode switched");
-        }
-        else
-        {
-          uint64_t u_id = p_modeExecutor->execute(goal->mode);
-          result.active_mode = p_modeExecutor->getExecutingMode();
-          result.active_priority = p_modeExecutor->getExecutingPriority();
-          result.track_id = u_id;
-          _as->setSucceeded(result, "Mode switched");
-        }
+        ROS_ERROR("Unsupported Color format. rgba values range is between 0.0 - 1.0");
+      }
+      else if (goal->mode.mode == cob_light::LightModes::NONE)
+      {
+        print_leds(goal->mode.colors);
+
+        p_modeExecutor->stop();
+        _color.a = 0;
+        p_colorO->setColor(_color);
+        result.active_mode = p_modeExecutor->getExecutingMode();
+        result.active_priority = p_modeExecutor->getExecutingPriority();
+        result.track_id = -1;
+        _as->setSucceeded(result, "Mode switched");
+      }
+      else
+      {
+        print_leds(goal->mode.colors);
+
+        uint64_t u_id = p_modeExecutor->execute(goal->mode);
+        result.active_mode = p_modeExecutor->getExecutingMode();
+        result.active_priority = p_modeExecutor->getExecutingPriority();
+        result.track_id = u_id;
+        _as->setSucceeded(result, "Mode switched");
+      }
     }
     else
-        _as->setAborted(result, "No color available");
+    {
+      _as->setAborted(result, "No color available");
+    }
   }
 
-  bool stopMode(cob_light::StopLightMode::Request &req, cob_light::StopLightMode::Response &res)
+  bool stopMode(cob_light::StopLightMode::Request& req, cob_light::StopLightMode::Response& res)
   {
-      boost::mutex::scoped_lock lock(_mutex);
-      bool ret = false;
-      ret = p_modeExecutor->stop(req.track_id);
-      res.active_mode = p_modeExecutor->getExecutingMode();
-      res.active_priority = p_modeExecutor->getExecutingPriority();
-      res.track_id = p_modeExecutor->getExecutingUId();
-      ret = true; // TODO: really check if mode is stopped
-      return ret;
+    boost::mutex::scoped_lock lock(_mutex);
+    bool ret = false;
+    ret = p_modeExecutor->stop(req.track_id);
+    res.active_mode = p_modeExecutor->getExecutingMode();
+    res.active_priority = p_modeExecutor->getExecutingPriority();
+    res.track_id = p_modeExecutor->getExecutingUId();
+    ret = true;  // TODO: really check if mode is stopped
+    return ret;
   }
 
   void publish_diagnostics_cb(const ros::TimerEvent&)
@@ -449,7 +479,7 @@ class LightControl
   ros::Timer _diagnostics_timer;
 
   typedef actionlib::SimpleActionServer<cob_light::SetLightModeAction> ActionServer;
-  ActionServer *_as;
+  ActionServer* _as;
 
   color::rgba _color;
 
@@ -463,16 +493,16 @@ class LightControl
 int main(int argc, char** argv)
 {
   // init node
-  ros::init(argc, argv, "light_controller", ros::init_options::NoSigintHandler);
-  signal(SIGINT, sigIntHandler);
+  ros::init(argc, argv, "cob_light");
+//  signal(SIGINT, sigIntHandler);
 
   // Override XMLRPC shutdown
-  ros::XMLRPCManager::instance()->unbind("shutdown");
-  ros::XMLRPCManager::instance()->bind("shutdown", shutdownCallback);
+//  ros::XMLRPCManager::instance()->unbind("shutdown");
+//  ros::XMLRPCManager::instance()->bind("shutdown", shutdownCallback);
 
   // create LightControl instance
-  LightControl *lightControl = new LightControl();
-  if(lightControl->init())
+  LightControl* lightControl = new LightControl();
+  if (lightControl->init())
   {
     ros::AsyncSpinner spinner(1);
     spinner.start();
diff --git a/cob_light/ros/src/colorOSim.cpp b/cob_light/ros/src/colorOSim.cpp
index 6d1e6f821..a1e6e61d8 100644
--- a/cob_light/ros/src/colorOSim.cpp
+++ b/cob_light/ros/src/colorOSim.cpp
@@ -21,14 +21,10 @@
 ColorOSim::ColorOSim(ros::NodeHandle* nh)
 {
   p_nh = nh;
-  _pubSimulation = p_nh->advertise<std_msgs::ColorRGBA>("debug",2);
+  _pubSimulation = p_nh->advertise<std_msgs::ColorRGBA>("debug", 2);
   _pubSimulationMulti = p_nh->advertise<cob_light::ColorRGBAArray>("debugMulti", 2);
 }
 
-ColorOSim::~ColorOSim()
-{
-}
-
 bool ColorOSim::init()
 {
   return true;
@@ -46,11 +42,11 @@ void ColorOSim::setColor(color::rgba color)
   m_sigColorSet(color);
 }
 
-void ColorOSim::setColorMulti(std::vector<color::rgba> &colors)
+void ColorOSim::setColorMulti(std::vector<color::rgba>& colors)
 {
   std_msgs::ColorRGBA color;
   cob_light::ColorRGBAArray colorMsg;
-  for(size_t i = 0; i < colors.size(); ++i)
+  for (size_t i = 0; i < colors.size(); ++i)
   {
     color.r = colors[i].r;
     color.g = colors[i].g;
@@ -62,3 +58,8 @@ void ColorOSim::setColorMulti(std::vector<color::rgba> &colors)
   _pubSimulationMulti.publish(colorMsg);
   m_sigColorSet(colors[0]);
 }
+
+bool equals(const float lhs, const float rhs, const float a_precision)
+{
+  return std::abs(lhs - rhs) <= a_precision;
+}
diff --git a/cob_light/ros/src/stageprofi.cpp b/cob_light/ros/src/stageprofi.cpp
index d5806146c..719b19e7c 100644
--- a/cob_light/ros/src/stageprofi.cpp
+++ b/cob_light/ros/src/stageprofi.cpp
@@ -54,24 +54,27 @@ bool StageProfi::init()
 
 void StageProfi::setColor(color::rgba color)
 {
+  ROS_WARN(__PRETTY_FUNCTION__);
   color::rgba color_tmp = color;
+  //color_tmp.a = 1.0;
+  //color_tmp.b = 1.0;
 
-  color_tmp.r *= color.a;
-  color_tmp.g *= color.a;
-  color_tmp.b *= color.a;
+  // color_tmp.r *= color.a;
+  // color_tmp.g *= color.a;
+  // color_tmp.b *= color.a;
 
-  color_tmp.r = fabs(color_tmp.r * 255);
-  color_tmp.g = fabs(color_tmp.g * 255);
-  color_tmp.b = fabs(color_tmp.b * 255);
+  // color_tmp.r = fabs(color_tmp.r * 255);
+  // color_tmp.g = fabs(color_tmp.g * 255);
+  // color_tmp.b = fabs(color_tmp.b * 255);
 
   unsigned int num_channels = _num_leds * 3;
   char channelbuffer[num_channels];
 
   for (int i = 0; i < _num_leds; i++)
   {
-    channelbuffer[i * 3] = (int) color_tmp.r;
-    channelbuffer[i * 3 + 1] = (int) color_tmp.g;
-    channelbuffer[i * 3 + 2] = (int) color_tmp.b;
+    channelbuffer[i * 3] = (char) (color_tmp.r*255);
+    channelbuffer[i * 3 + 1]= (char) (color_tmp.g*255);
+    channelbuffer[i * 3 + 2]= (char) (color_tmp.b*255);
   }
 
   uint16_t index = 0;
@@ -79,6 +82,7 @@ void StageProfi::setColor(color::rgba color)
   {
     unsigned int size = std::min((unsigned int) MAX_CHANNELS,
         num_channels - index);
+        ROS_WARN_STREAM("SENDING DATA: channelbuffer@"<<(index)<<" size: "<<size<<" num_channgels: "<<num_channels);
     if (sendDMX(index, channelbuffer + index, size))
       index += size;
     else
@@ -93,6 +97,7 @@ void StageProfi::setColor(color::rgba color)
 
 void StageProfi::setColorMulti(std::vector<color::rgba> &colors)
 {
+  ROS_WARN(__PRETTY_FUNCTION__);
   color::rgba color_tmp;
   unsigned int num_channels = _num_leds * 3;
   char channelbuffer[num_channels];