From 04492f0bf6c571f8621c7aa9909293897a78571e Mon Sep 17 00:00:00 2001
From: Roni Kreinin <59886299+roni-kreinin@users.noreply.github.com>
Date: Thu, 16 Jan 2025 16:22:01 -0500
Subject: [PATCH] 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
---
 .../lighting/color.hpp                        | 26 ++++++++++++++++---
 .../lighting/sequence.hpp                     | 23 ++++++++++++++++
 .../src/lighting/lighting.cpp                 |  2 +-
 3 files changed, 47 insertions(+), 4 deletions(-)

diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/color.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/color.hpp
index ce005a9..0d85a28 100644
--- a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/color.hpp
+++ b/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_;
 };
diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/sequence.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/sequence.hpp
index ba10f69..8df095f 100644
--- a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/sequence.hpp
+++ b/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_;
diff --git a/clearpath_hardware_interfaces/src/lighting/lighting.cpp b/clearpath_hardware_interfaces/src/lighting/lighting.cpp
index 5cd91be..53452ee 100644
--- a/clearpath_hardware_interfaces/src/lighting/lighting.cpp
+++ b/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();