Fixed motor error lighting sequence (#120)

* Update motor error lighting sequence if motor_states changes without the lighting state changing
Added lighting emulator script

* Removed light emulator

clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/color.hpp

@@ -60,6 +60,16 @@ struct hsv_t {
     s = 0.0;
     v = 0.0;
   }
+
+  bool operator==(hsv_t const& other)
+  {
+    return (h == other.h && s == other.s && v == other.v);
+  }
+
+  bool operator!=(hsv_t const& other)
+  {
+    return !(*this == other);
+  }
 };
 
 static const hsv_t COLOR_RED = hsv_t(0.0, 100.0, 37.5);
@@ -85,9 +95,9 @@ class ColorHSV
   ColorHSV(hsv_t hsv);
   static std::vector<ColorHSV> fade(ColorHSV start, ColorHSV end, uint32_t steps);
   clearpath_platform_msgs::msg::RGB getRgbMsg();
-  double h() { return hsv_.h; };
-  double s() { return hsv_.s; };
-  double v() { return hsv_.v; };
+  double h() const { return hsv_.h; };
+  double s() const { return hsv_.s; };
+  double v() const { return hsv_.v; };
 
   void setH(const double h)
   {
@@ -104,6 +114,16 @@ class ColorHSV
     hsv_.v = v;
   }
 
+  bool operator==(const ColorHSV other) const
+  {
+    return (h() == other.h() && s() == other.s() && v() == other.v());
+  }
+
+  bool operator!=(const ColorHSV other) const
+  {
+    return !(*this == other);
+  }
+
 private:
   hsv_t hsv_;
 };

clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/sequence.hpp

@@ -94,6 +94,29 @@ class Sequence
     return s;
   }
 
+  bool operator==(Sequence const& other)
+  {
+    LightingSequence ls, other_ls;
+
+    ls = sequence_;
+    other_ls = other.getSequence();
+
+    for (std::size_t i = 0; i < sequence_.size(); i++)
+    {
+      if (ls.at(i) != other_ls.at(i))
+      {
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  bool operator!=(Sequence const& other)
+  {
+    return !(sequence_ == other.getSequence());
+  }
+
 protected:
   LightingSequence sequence_;
   uint16_t current_state_, num_states_;

clearpath_hardware_interfaces/src/lighting/lighting.cpp

@@ -178,7 +178,7 @@ void Lighting::spinOnce()
   }
 
   // Change lighting sequence if state has changed
-  if (old_state_ != state_)
+  if (old_state_ != state_ || current_sequence_ != lighting_sequence_.at(old_state_))
   {
     current_sequence_ = lighting_sequence_.at(state_);
     current_sequence_.reset();