From 22499989de1ae5730e0e290fcfb63e8abbbea238 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Wed, 28 Aug 2024 11:51:20 +0200
Subject: [PATCH] LEDStrip plugin to Gazebo (#391)

* Save work

* save work

* Save work

* Fix light

* Format

* Clean up

* Clean up

* Documentation

* Simplify

* Rename folder

* Clean up

* pre-commit

* Add simple brief

* Review minor changes

* Jakub suggestions

* Prevent from initial error and warn logs

* Improve light property

* Add suggestions

* Add suggestions

* Throw instead of errormsg
---
 panther_battery/README.md                     |   2 +-
 panther_description/urdf/gazebo.urdf.xacro    |  21 +-
 .../urdf/panther_macro.urdf.xacro             |   4 +-
 panther_gazebo/CMakeLists.txt                 |  47 +--
 panther_gazebo/README.md                      |  27 +-
 panther_gazebo/config/gz_bridge.yaml          |   6 +-
 panther_gazebo/config/led_strips.yaml         |  21 --
 panther_gazebo/hooks/panther_gazebo.sh.in     |   1 +
 .../{plugins => gui}/e_stop.hpp               |   6 +-
 .../include/panther_gazebo/gz_led_strip.hpp   | 109 -------
 .../panther_gazebo/gz_led_strip_manager.hpp   |  49 ---
 .../include/panther_gazebo/led_strip.hpp      | 149 +++++++++
 .../launch/simulate_robot.launch.py           |   8 -
 panther_gazebo/launch/simulation.launch.py    |   2 +-
 panther_gazebo/package.xml                    |   7 +-
 panther_gazebo/src/{plugins => gui}/EStop.qml |   0
 panther_gazebo/src/{plugins => gui}/EStop.qrc |   0
 .../src/{plugins => gui}/e_stop.cpp           |   2 +-
 panther_gazebo/src/gz_led_strip.cpp           | 215 -------------
 panther_gazebo/src/gz_led_strip_manager.cpp   |  68 -----
 panther_gazebo/src/led_strip.cpp              | 282 ++++++++++++++++++
 panther_gazebo/src/main.cpp                   |  48 ---
 22 files changed, 517 insertions(+), 557 deletions(-)
 delete mode 100644 panther_gazebo/config/led_strips.yaml
 rename panther_gazebo/include/panther_gazebo/{plugins => gui}/e_stop.hpp (93%)
 delete mode 100644 panther_gazebo/include/panther_gazebo/gz_led_strip.hpp
 delete mode 100644 panther_gazebo/include/panther_gazebo/gz_led_strip_manager.hpp
 create mode 100644 panther_gazebo/include/panther_gazebo/led_strip.hpp
 rename panther_gazebo/src/{plugins => gui}/EStop.qml (100%)
 rename panther_gazebo/src/{plugins => gui}/EStop.qrc (100%)
 rename panther_gazebo/src/{plugins => gui}/e_stop.cpp (98%)
 delete mode 100644 panther_gazebo/src/gz_led_strip.cpp
 delete mode 100644 panther_gazebo/src/gz_led_strip_manager.cpp
 create mode 100644 panther_gazebo/src/led_strip.cpp
 delete mode 100644 panther_gazebo/src/main.cpp

diff --git a/panther_battery/README.md b/panther_battery/README.md
index 6b0e8b447..dc151104f 100644
--- a/panther_battery/README.md
+++ b/panther_battery/README.md
@@ -22,7 +22,7 @@ Publishes battery state read from ADC unit for Panther version 1.2 and above, or
 - `battery/charging_status` [*panther_msgs/ChargingStatus*]: Battery charging status.
 - `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Battery diagnostic messages.
 
-#### Subscribes
+#### Subscribers
 
 - `hardware/io_state` [*panther_msgs/IOState*]: Current state of IO.
 - `hardware/motor_controllers_state` [*panther_msgs/DriverState*]: Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.
diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro
index e34de17a8..29e3558f0 100644
--- a/panther_description/urdf/gazebo.urdf.xacro
+++ b/panther_description/urdf/gazebo.urdf.xacro
@@ -106,9 +106,18 @@
     </gazebo>
   </xacro:macro>
 
-
   <!-- Lights -->
-  <xacro:macro name="light" params="reference_frame name namespace:=''">
+  <xacro:macro name="light" params="reference_frame name topic namespace:=''">
+    <gazebo>
+      <plugin filename="LEDStrip" name="panther_gazebo::LEDStrip">
+        <light_name>${name}</light_name>
+        <topic>${topic}</topic>
+        <namespace>${namespace}</namespace>
+        <frequency>10</frequency>
+        <width>0.5</width>
+        <height>0.015</height>
+      </plugin>
+    </gazebo>
     <gazebo reference="${reference_frame}">
       <light name="${name}" type="spot">
         <cast_shadows>true</cast_shadows>
@@ -116,12 +125,18 @@
         <diffuse>1.0 1.0 1.0</diffuse>
         <specular>1.0 1.0 1.0</specular>
         <direction>1 0 0</direction>
-        <intensity>1.0</intensity>
+        <intensity>0.5</intensity>
         <spot>
           <inner_angle>1.0</inner_angle>
           <outer_angle>2.0</outer_angle>
           <falloff>0.4</falloff>
         </spot>
+        <attenuation>
+          <range>20.0</range>
+          <constant>1.0</constant>
+          <linear>1.0</linear>
+          <quadratic>1.0</quadratic>
+        </attenuation>
       </light>
     </gazebo>
   </xacro:macro>
diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro
index fc6da39ee..8d2a83207 100644
--- a/panther_description/urdf/panther_macro.urdf.xacro
+++ b/panther_description/urdf/panther_macro.urdf.xacro
@@ -185,8 +185,8 @@
       </xacro:if>
       <xacro:gazebo.controller config_file="${controller_config_file}" namespace="${namespace}" />
       <xacro:gazebo.imu reference_frame="imu_link" namespace="${namespace}" />
-      <xacro:gazebo.light reference_frame="lights_channel_1_link" name="lights_channel_1" namespace="${namespace}" />
-      <xacro:gazebo.light reference_frame="lights_channel_2_link" name="lights_channel_2" namespace="${namespace}" />
+      <xacro:gazebo.light reference_frame="lights_channel_1_link" name="lights_channel_1" topic="lights/channel_1_frame" namespace="${namespace}" />
+      <xacro:gazebo.light reference_frame="lights_channel_2_link" name="lights_channel_2" topic="lights/channel_2_frame" namespace="${namespace}" />
     </xacro:if>
   </xacro:macro>
 
diff --git a/panther_gazebo/CMakeLists.txt b/panther_gazebo/CMakeLists.txt
index 31dc77d04..cae0aaa39 100644
--- a/panther_gazebo/CMakeLists.txt
+++ b/panther_gazebo/CMakeLists.txt
@@ -8,18 +8,16 @@ endif()
 set(PACKAGE_DEPENDENCIES
     ament_cmake
     hardware_interface
-    ignition-common4
     ignition-gazebo6
     ignition-msgs8
     ignition-plugin1
     ignition-transport11
     ign_ros2_control
     pluginlib
-    panther_utils
     rclcpp
+    realtime_tools
     std_msgs
-    std_srvs
-    yaml-cpp)
+    std_srvs)
 
 foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
   find_package(${PACKAGE} REQUIRED)
@@ -30,13 +28,6 @@ find_package(Qt5 REQUIRED COMPONENTS Core Quick QuickControls2)
 include_directories(include ${Qt5Core_INCLUDE_DIRS} ${Qt5Qml_INCLUDE_DIRS}
                     ${Qt5Quick_INCLUDE_DIRS} ${Qt5QuickControls2_INCLUDE_DIRS})
 
-add_executable(gz_led_strip_manager src/main.cpp src/gz_led_strip_manager.cpp
-                                    src/gz_led_strip.cpp)
-ament_target_dependencies(gz_led_strip_manager panther_utils)
-target_link_libraries(
-  gz_led_strip_manager ignition-transport11::core ignition-msgs8
-  ignition-common4::ignition-common4 yaml-cpp)
-
 add_library(panther_simulation_plugins SHARED src/gz_panther_system.cpp)
 ament_target_dependencies(
   panther_simulation_plugins
@@ -46,19 +37,27 @@ ament_target_dependencies(
   rclcpp_lifecycle
   std_msgs
   std_srvs)
-target_link_libraries(panther_simulation_plugins ignition-gazebo6::core)
+target_link_libraries(panther_simulation_plugins ignition-gazebo6)
 
 set(CMAKE_AUTOMOC ON)
-qt5_add_resources(resources_rcc src/plugins/EStop.qrc)
-
-add_library(EStop SHARED include/panther_gazebo/plugins/e_stop.hpp
-                         src/plugins/e_stop.cpp ${resources_rcc})
-ament_target_dependencies(EStop ignition-common4 ignition-gazebo6
-                          ignition-plugin1 rclcpp std_srvs)
-target_link_libraries(EStop ${Qt5Core_LIBRARIES} ${Qt5Qml_LIBRARIES}
-                      ${Qt5Quick_LIBRARIES} ${Qt5QuickControls2_LIBRARIES})
+qt5_add_resources(resources_rcc src/gui/EStop.qrc)
 
-install(TARGETS gz_led_strip_manager DESTINATION lib/${PROJECT_NAME})
+add_library(EStop SHARED include/panther_gazebo/gui/e_stop.hpp
+                         src/gui/e_stop.cpp ${resources_rcc})
+ament_target_dependencies(EStop rclcpp std_srvs)
+target_link_libraries(
+  EStop
+  ignition-gazebo6
+  ignition-plugin1
+  ${Qt5Core_LIBRARIES}
+  ${Qt5Qml_LIBRARIES}
+  ${Qt5Quick_LIBRARIES}
+  ${Qt5QuickControls2_LIBRARIES})
+
+add_library(LEDStrip SHARED src/led_strip.cpp)
+ament_target_dependencies(LEDStrip realtime_tools)
+target_link_libraries(LEDStrip ignition-gazebo6 ignition-msgs8 ignition-plugin1
+                      ignition-transport11)
 
 install(
   TARGETS panther_simulation_plugins
@@ -72,6 +71,12 @@ install(
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib)
 
+install(
+  TARGETS LEDStrip
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib)
+
 install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)
diff --git a/panther_gazebo/README.md b/panther_gazebo/README.md
index 36bf7bb30..a4d3bb85a 100644
--- a/panther_gazebo/README.md
+++ b/panther_gazebo/README.md
@@ -13,7 +13,6 @@ The package contains a launch file and source files used to run the robot simula
 
 - [`battery_plugin_config.yaml`](./config/battery_plugin_config.yaml): Simulated LinearBatteryPlugin configuration.
 - [`gz_bridge.yaml`](./config/gz_bridge.yaml): Specify data to exchange between ROS and Gazebo simulation.
-- [`led_strips.yaml`](./config/led_strips.yaml): Configure properties of led strips in simulation to animate lights.
 - [`teleop_with_estop.config`](./config/teleop_with_estop.config): Gazebo layout configuration file, which adds E-Stop and Teleop widgets.
 
 ## ROS Nodes
@@ -48,3 +47,29 @@ Plugin based on `ign_system` is responsible for handling sensor interfaces (only
 Required parameters are defined when including the interface in the URDF (you can check out [panther_macro.urdf.xacro](../panther_description/urdf/panther_macro.urdf.xacro)).
 
 - `e_stop_initial_state` [*bool*, default: **true**]: Initial state of E-stop.
+
+### LEDStrip
+
+`LEDStrip` is a Gazebo System plugin responsible for visualizing light and displaying markers based on the data received from a `gz::msgs::Image` message.
+
+> [!NOTE]
+> The topics and services mentioned below are related to Gazebo interfaces, not ROS interfaces.
+
+#### Subscribers
+
+- `{topic}` [*gz::msgs::Image*]: Subscribes to an image message for visualization. The topic is specified via a parameter.
+
+#### Service Servers
+
+- `/marker` [*gz::msgs::Marker*]: Service to request markers for visualizing the received image.
+
+#### Parameters
+
+The following parameters are required when including this interface in the URDF (you can refer to the [gazebo.urdf.xacro](../panther_description/urdf/gazebo.urdf.xacro) file for details).
+
+- `light_name` [*string*, default: **""**]: The name of the light entity. The visualization will be attached to this entity.
+- `topic` [*string*, default: **""**]: The name of the topic from which the Image message is received.
+- `namespace` [*string*, default: **""**]: Specifies the namespace to differentiate topics and models in scenarios with multiple robots.
+- `frequency` [*double*, default: **10.0**]: Defines the frequency at which the animation is updated.
+- `width` [*double*, default: **1.0**]: Specifies the width (y-axis) of the visualization array.
+- `height` [*double*, default: **1.0**]: Specifies the height (z-axis) of the visualization array.
diff --git a/panther_gazebo/config/gz_bridge.yaml b/panther_gazebo/config/gz_bridge.yaml
index 8b6c2328c..d7f8114bf 100644
--- a/panther_gazebo/config/gz_bridge.yaml
+++ b/panther_gazebo/config/gz_bridge.yaml
@@ -17,12 +17,14 @@
   gz_type_name: ignition.msgs.Twist
   direction: GZ_TO_ROS
 
-- topic_name: lights/channel_1_frame
+- ros_topic_name: lights/channel_1_frame
+  gz_topic_name: <namespace>/lights/channel_1_frame
   ros_type_name: sensor_msgs/msg/Image
   gz_type_name: ignition.msgs.Image
   direction: ROS_TO_GZ
 
-- topic_name: lights/channel_2_frame
+- ros_topic_name: lights/channel_2_frame
+  gz_topic_name: <namespace>/lights/channel_2_frame
   ros_type_name: sensor_msgs/msg/Image
   gz_type_name: ignition.msgs.Image
   direction: ROS_TO_GZ
diff --git a/panther_gazebo/config/led_strips.yaml b/panther_gazebo/config/led_strips.yaml
deleted file mode 100644
index c23e06e5f..000000000
--- a/panther_gazebo/config/led_strips.yaml
+++ /dev/null
@@ -1,21 +0,0 @@
-chanel_1:
-  frequency: 12  # Setting to high frequency caused lags in the simulation
-  world_name: husarion_world
-  parent_link: panther
-  position: [0.362, 0.0, 0.201]
-  orientation: [0.0, 0.0, 0.0]
-  led_strip_width: 0.5
-  topic: lights/channel_1_frame
-  light_name: lights_channel_1
-  number_of_leds: 46
-
-chanel_2:
-  frequency: 12  # Setting to high frequency caused lags in the simulation
-  world_name: husarion_world
-  parent_link: panther
-  position: [-0.362, 0.0, 0.201]
-  orientation: [0.0, 0.0, 0.0]
-  led_strip_width: 0.5
-  topic: lights/channel_2_frame
-  light_name: lights_channel_2
-  number_of_leds: 46
diff --git a/panther_gazebo/hooks/panther_gazebo.sh.in b/panther_gazebo/hooks/panther_gazebo.sh.in
index 2aaf5d29b..5e75c5eb0 100644
--- a/panther_gazebo/hooks/panther_gazebo.sh.in
+++ b/panther_gazebo/hooks/panther_gazebo.sh.in
@@ -1 +1,2 @@
 ament_prepend_unique_value IGN_GUI_PLUGIN_PATH "@CMAKE_INSTALL_PREFIX@/lib"
+ament_prepend_unique_value IGN_GAZEBO_SYSTEM_PLUGIN_PATH= "@CMAKE_INSTALL_PREFIX@/lib"
diff --git a/panther_gazebo/include/panther_gazebo/plugins/e_stop.hpp b/panther_gazebo/include/panther_gazebo/gui/e_stop.hpp
similarity index 93%
rename from panther_gazebo/include/panther_gazebo/plugins/e_stop.hpp
rename to panther_gazebo/include/panther_gazebo/gui/e_stop.hpp
index 2edc75636..055db7e16 100644
--- a/panther_gazebo/include/panther_gazebo/plugins/e_stop.hpp
+++ b/panther_gazebo/include/panther_gazebo/gui/e_stop.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_GAZEBO_PLUGINS_ESTOP_HPP_
-#define PANTHER_GAZEBO_PLUGINS_ESTOP_HPP_
+#ifndef PANTHER_GAZEBO_GUI_E_STOP_HPP_
+#define PANTHER_GAZEBO_GUI_E_STOP_HPP_
 
 #include <string>
 
@@ -61,4 +61,4 @@ protected slots:
 };
 }  // namespace panther_gazebo
 
-#endif  // PANTHER_GAZEBO_PLUGINS_ESTOP_HPP_
+#endif  // PANTHER_GAZEBO_GUI_E_STOP_HPP_
diff --git a/panther_gazebo/include/panther_gazebo/gz_led_strip.hpp b/panther_gazebo/include/panther_gazebo/gz_led_strip.hpp
deleted file mode 100644
index cf177ff8f..000000000
--- a/panther_gazebo/include/panther_gazebo/gz_led_strip.hpp
+++ /dev/null
@@ -1,109 +0,0 @@
-// Copyright 2024 Husarion sp. z o.o.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef PANTHER_GAZEBO_GZ_LED_STRIP_HPP_
-#define PANTHER_GAZEBO_GZ_LED_STRIP_HPP_
-
-#include <chrono>
-#include <memory>
-#include <string>
-#include <vector>
-
-#include <yaml-cpp/yaml.h>
-
-#include <gz/common/Time.hh>
-#include <gz/math.hh>
-#include <gz/msgs.hh>
-#include <gz/transport.hh>
-
-using namespace std::chrono_literals;
-
-/**
- * @brief Structure to hold properties of each LED channel
- */
-struct ChannelProperties
-{
-  uint8_t frequency;
-  std::string world_name;
-  std::string parent_link;
-  std::vector<double> position;
-  std::vector<double> orientation;
-  double led_strip_width;
-  std::string topic;
-  std::string light_name;
-  unsigned int number_of_leds;
-};
-
-struct RGBAColor
-{
-  float r;
-  float g;
-  float b;
-  float a;
-};
-
-/**
- * @brief Class to manage an LED strip in a Gazebo simulation
- */
-class LEDStrip
-{
-public:
-  /**
-   * @brief Construct a new LEDStrip object
-   *
-   * @param channel_properties Properties of the LED channel
-   */
-  LEDStrip(ChannelProperties channel_properties);
-  ~LEDStrip();
-
-private:
-  void ImageCallback(const gz::msgs::Image & msg);
-  void MsgValidation(const gz::msgs::Image & msg);
-
-  /**
-   * @brief Manage color of robot simulated lights based on the image message
-   */
-  void ManageLights(const gz::msgs::Image & msg);
-
-  /**
-   * @brief Manage color of robot LED strip based on the image message
-   */
-  void ManageVisualization(const gz::msgs::Image & msg);
-  RGBAColor CalculateMeanRGBA(const gz::msgs::Image & msg);
-
-  /**
-   * @brief Sending a message to change the color of the light
-   *
-   * @param rgba The RGBA color to publish
-   */
-  void PublishLight(const RGBAColor & rgba);
-
-  /**
-   * @brief Create a marker element (single LED from LED Strip)
-   *
-   * @param marker The pointer to marker to create
-   * @param id The unique ID of the marker (if not unique, the marker will replace the existing one)
-   */
-  void CreateMarker(ignition::msgs::Marker * marker, const int id);
-  void SetMarkerColor(gz::msgs::Marker * marker, const RGBAColor & rgba);
-
-  static unsigned int first_free_available_marker_idx_;
-  const int first_led_marker_idx_;
-
-  ChannelProperties channel_properties_;
-  std::shared_ptr<gz::transport::Node> node_;
-  std::shared_ptr<gz::transport::Node::Publisher> light_pub_;
-};
-
-#endif  // PANTHER_GAZEBO_GZ_LED_STRIP_HPP_
diff --git a/panther_gazebo/include/panther_gazebo/gz_led_strip_manager.hpp b/panther_gazebo/include/panther_gazebo/gz_led_strip_manager.hpp
deleted file mode 100644
index 3494ed580..000000000
--- a/panther_gazebo/include/panther_gazebo/gz_led_strip_manager.hpp
+++ /dev/null
@@ -1,49 +0,0 @@
-// Copyright 2024 Husarion sp. z o.o.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef PANTHER_GAZEBO_GZ_LED_STRIP_MANAGER_HPP_
-#define PANTHER_GAZEBO_GZ_LED_STRIP_MANAGER_HPP_
-
-#include <list>
-#include <string>
-
-#include <yaml-cpp/yaml.h>
-
-#include "panther_gazebo/gz_led_strip.hpp"
-#include "panther_utils/yaml_utils.hpp"
-
-/**
- * @brief Class to manage multiple LED strip object in Gazebo simulation based on provided
- * configuration
- */
-class LEDStripManager
-{
-public:
-  explicit LEDStripManager(const std::string & config_file);
-  void LoadConfig(const std::string & config_file);
-
-  /**
-   * @brief Create as many LED strips as defined in the configuration file.
-   */
-  void CreateLEDStrips();
-
-  template <typename T>
-  void AssignProperty(const YAML::Node & channel_args, T & property, const std::string & key);
-
-private:
-  YAML::Node config_;
-  std::list<LEDStrip> led_strips_;
-};
-
-#endif  // PANTHER_GAZEBO_GZ_LED_STRIP_MANAGER_HPP_
diff --git a/panther_gazebo/include/panther_gazebo/led_strip.hpp b/panther_gazebo/include/panther_gazebo/led_strip.hpp
new file mode 100644
index 000000000..0f0945ea6
--- /dev/null
+++ b/panther_gazebo/include/panther_gazebo/led_strip.hpp
@@ -0,0 +1,149 @@
+// Copyright 2024 Husarion sp. z o.o.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PANTHER_GAZEBO_LED_STRIP_HPP_
+#define PANTHER_GAZEBO_LED_STRIP_HPP_
+
+#include <chrono>
+#include <string>
+
+#include <realtime_tools/realtime_box.h>
+
+#include <gz/math/Color.hh>
+#include <gz/math/Pose3.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
+#include <gz/sim/System.hh>
+#include <gz/sim/components/Light.hh>
+#include <gz/sim/components/Pose.hh>
+#include <gz/transport/Node.hh>
+#include <sdf/Element.hh>
+
+#include <gz/msgs/image.pb.h>
+#include <gz/msgs/light.pb.h>
+
+namespace panther_gazebo
+{
+
+/**
+ * @brief Class to manage an LED strip in a Gazebo simulation based on received image.
+ */
+class LEDStrip : public gz::sim::System,
+                 public gz::sim::ISystemConfigure,
+                 public gz::sim::ISystemPreUpdate
+{
+public:
+  /**
+   * @brief Configures the LED strip. This function fill up parameters and light_cmd_ based on URDF.
+   * Inherit from gz::sim::ISystemConfigure. More information can be found in the [Gazebo
+   * documentation](https://gazebosim.org/api/gazebo/6/createsystemplugins.html).
+   *
+   * @param id The entity ID of the model.
+   * @param sdf The SDF element of the model.
+   * @param ecm The entity component manager.
+   * @param eventMgr The event manager.
+   *
+   * @exception std::runtime_error if the entity is not a model.
+   */
+  void Configure(
+    const gz::sim::Entity & id, const std::shared_ptr<const sdf::Element> & sdf,
+    gz::sim::EntityComponentManager & ecm, gz::sim::EventManager & eventMgr) override;
+
+  /**
+   * @brief Displays lights and markers, with specified by URDF frequency. Inherit from
+   * gz::sim::ISystemPreUpdate. More information can be found in the [Gazebo
+   * documentation](https://gazebosim.org/api/gazebo/6/createsystemplugins.html).
+   *
+   * @param info The update information.
+   * @param ecm The entity component manager.
+   */
+  void PreUpdate(const gz::sim::UpdateInfo & info, gz::sim::EntityComponentManager & ecm) override;
+
+private:
+  /**
+   * @brief Parse parameters from the URDF file
+   *
+   * @param sdf The SDF element of the model.
+   * @exception std::runtime_error if the light_name or topic parameter is missing.
+   */
+  void ParseParameters(const std::shared_ptr<const sdf::Element> & sdf);
+
+  /**
+   * @brief Return Light command based on light configuration specified in URDF file Light
+   * properties
+   *
+   * @param ecm The entity component manager.
+   * @return Light command message
+   * @exception std::runtime_error if the light entity is not found.
+   */
+  gz::msgs::Light SetupLightCmd(gz::sim::EntityComponentManager & ecm);
+
+  /**
+   * @brief Convert sdf::Light (configuration from URDF) to gz::msgs::Light (command msg)
+   *
+   * @param light_sdf Light SDF configuration
+   * @return Light command message
+   */
+  gz::msgs::Light CreateLightMsgFromSdf(const sdf::Light & light_sdf);
+  void ImageCallback(const gz::msgs::Image & msg);
+  bool IsEncodingValid(const gz::msgs::Image & msg);
+  gz::math::Color CalculateMeanColor(const gz::msgs::Image & msg);
+
+  /**
+   * @brief Manage color of robot simulated lights based on the image message
+   *
+   * @param ecm The entity component manager.
+   * @param image The image message
+   */
+  void VisualizeLights(gz::sim::EntityComponentManager & ecm, const gz::msgs::Image & image);
+
+  /**
+   * @brief Manage color of robot LED strip based on the image message
+   *
+   * @param image The image message
+   * @param light_pose The pose of the light
+   */
+  void VisualizeMarkers(const gz::msgs::Image & image, const gz::math::Pose3d & light_pose);
+
+  /**
+   * @brief Create a marker element (single LED from LED Strip)
+   *
+   * @param id The unique ID of the marker (if not unique, the marker will replace the existing one)
+   * @param pose The pose of the marker
+   * @param color The color of the marker
+   * @param size The size of the marker
+   */
+  void CreateMarker(
+    const uint id, const gz::math::Pose3d pose, const gz::math::Color & color,
+    const gz::math::Vector3d size);
+
+  std::string light_name_;
+  std::string image_topic_;
+  std::string ns_ = "";
+  double frequency_ = 10.0;
+  double marker_width_ = 1.0;
+  double marker_height_ = 1.0;
+
+  bool new_image_available_ = false;
+  gz::msgs::Light light_cmd_;
+  realtime_tools::RealtimeBox<gz::msgs::Image> last_image_;
+  gz::sim::Entity light_entity_{gz::sim::kNullEntity};
+  gz::transport::Node node_;
+  std::chrono::steady_clock::duration last_update_time_{std::chrono::seconds(
+    1)};  // Avoid initialization errors when the robot is not yet spawned on the scene.
+};
+
+}  // namespace panther_gazebo
+
+#endif  // PANTHER_GAZEBO_LED_STRIP_HPP_
diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py
index 55d120be8..ad7252e7a 100644
--- a/panther_gazebo/launch/simulate_robot.launch.py
+++ b/panther_gazebo/launch/simulate_robot.launch.py
@@ -121,13 +121,6 @@ def generate_launch_description():
         condition=UnlessCondition(PythonExpression(["'", namespace, "' == ''"])),
     )
 
-    gz_led_strip_manager = Node(
-        package="panther_gazebo",
-        executable="gz_led_strip_manager",
-        namespace=namespace,
-        arguments=["--config-file", gz_led_strip_config],
-    )
-
     controller_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -203,7 +196,6 @@ def generate_launch_description():
             spawn_robot_launch,
             lights_launch,
             manager_launch,
-            gz_led_strip_manager,
             controller_launch,
             ekf_launch,
             simulate_components,
diff --git a/panther_gazebo/launch/simulation.launch.py b/panther_gazebo/launch/simulation.launch.py
index 348fbaee6..04c192d74 100644
--- a/panther_gazebo/launch/simulation.launch.py
+++ b/panther_gazebo/launch/simulation.launch.py
@@ -56,7 +56,7 @@ def generate_launch_description():
                 [FindPackageShare("husarion_gz_worlds"), "launch", "gz_sim.launch.py"]
             )
         ),
-        launch_arguments={"gz_gui": namespaced_gz_gui}.items(),
+        launch_arguments={"gz_gui": namespaced_gz_gui, "gz_log_level": "1"}.items(),
     )
 
     simulate_robots = IncludeLaunchDescription(
diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml
index af1ab5412..89e7baaac 100644
--- a/panther_gazebo/package.xml
+++ b/panther_gazebo/package.xml
@@ -19,14 +19,13 @@
 
   <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">hardware_interface</depend>
   <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">ign_ros2_control</depend>
-  <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_utils</depend>
   <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">pluginlib</depend>
   <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">rclcpp</depend>
   <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">rclcpp_lifecycle</depend>
-  <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">sensor_msgs</depend>
+  <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">realtime_tools</depend>
+  <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">ros_gz_sim</depend>
   <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">std_msgs</depend>
   <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">std_srvs</depend>
-  <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">yaml-cpp</depend>
 
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">controller_manager</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">husarion_gz_worlds</exec_depend>
@@ -38,10 +37,10 @@
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_lights</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_localization</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_manager</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_utils</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">robot_state_publisher</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">ros_components_description</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">ros_gz_bridge</exec_depend>
-  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">ros_gz_sim</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">tf2_ros</exec_depend>
 
   <!-- Ignition dependency is specified in the ros_gz_sim package,
diff --git a/panther_gazebo/src/plugins/EStop.qml b/panther_gazebo/src/gui/EStop.qml
similarity index 100%
rename from panther_gazebo/src/plugins/EStop.qml
rename to panther_gazebo/src/gui/EStop.qml
diff --git a/panther_gazebo/src/plugins/EStop.qrc b/panther_gazebo/src/gui/EStop.qrc
similarity index 100%
rename from panther_gazebo/src/plugins/EStop.qrc
rename to panther_gazebo/src/gui/EStop.qrc
diff --git a/panther_gazebo/src/plugins/e_stop.cpp b/panther_gazebo/src/gui/e_stop.cpp
similarity index 98%
rename from panther_gazebo/src/plugins/e_stop.cpp
rename to panther_gazebo/src/gui/e_stop.cpp
index c11428d66..ff4c30102 100644
--- a/panther_gazebo/src/plugins/e_stop.cpp
+++ b/panther_gazebo/src/gui/e_stop.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_gazebo/plugins/e_stop.hpp"
+#include "panther_gazebo/gui/e_stop.hpp"
 
 #include <memory>
 
diff --git a/panther_gazebo/src/gz_led_strip.cpp b/panther_gazebo/src/gz_led_strip.cpp
deleted file mode 100644
index d0eb632b8..000000000
--- a/panther_gazebo/src/gz_led_strip.cpp
+++ /dev/null
@@ -1,215 +0,0 @@
-// Copyright 2024 Husarion sp. z o.o.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "panther_gazebo/gz_led_strip.hpp"
-
-#include <algorithm>
-#include <exception>
-#include <iostream>
-#include <memory>
-#include <stdexcept>
-#include <string>
-#include <vector>
-
-#include <yaml-cpp/yaml.h>
-
-#include <gz/math.hh>
-#include <gz/msgs.hh>
-#include <gz/transport.hh>
-
-unsigned int LEDStrip::first_free_available_marker_idx_ =
-  1;  // The marker with id 0 is not modified.
-
-LEDStrip::LEDStrip(ChannelProperties channel_properties)
-: first_led_marker_idx_(first_free_available_marker_idx_), channel_properties_(channel_properties)
-{
-  first_free_available_marker_idx_ += channel_properties_.number_of_leds;
-  gz::transport::SubscribeOptions opts;
-  node_ = std::make_shared<gz::transport::Node>();
-  opts.SetMsgsPerSec(channel_properties_.frequency);
-  node_->Subscribe(channel_properties_.topic, &LEDStrip::ImageCallback, this, opts);
-
-  light_pub_ = std::make_shared<gz::transport::Node::Publisher>(node_->Advertise<gz::msgs::Light>(
-    "/world/" + channel_properties_.world_name + "/light_config"));
-}
-
-LEDStrip::~LEDStrip() { first_free_available_marker_idx_ -= channel_properties_.number_of_leds; }
-
-void LEDStrip::ImageCallback(const gz::msgs::Image & msg)
-{
-  try {
-    MsgValidation(msg);
-  } catch (const std::runtime_error & e) {
-    std::cerr << "[Error] with " << channel_properties_.light_name << ": " << e.what() << std::endl;
-    return;
-  }
-
-  ManageLights(msg);
-  ManageVisualization(msg);
-}
-
-void LEDStrip::MsgValidation(const gz::msgs::Image & msg)
-{
-  if (
-    msg.pixel_format_type() != gz::msgs::PixelFormatType::RGBA_INT8 &&
-    msg.pixel_format_type() != gz::msgs::PixelFormatType::RGB_INT8) {
-    throw std::runtime_error("Incorrect image encoding");
-  }
-
-  if (msg.width() != channel_properties_.number_of_leds || msg.height() != 1) {
-    throw std::runtime_error(
-      "Image dimensions are incorrect. \nExpected width: " +
-      std::to_string(channel_properties_.number_of_leds) + ", height: 1. \nReceived width: " +
-      std::to_string(msg.width()) + ", height: " + std::to_string(msg.height()));
-  }
-}
-
-void LEDStrip::ManageLights(const gz::msgs::Image & msg)
-{
-  const auto mean_rgba = CalculateMeanRGBA(msg);
-  PublishLight(mean_rgba);
-}
-
-void LEDStrip::ManageVisualization(const gz::msgs::Image & msg)
-{
-  gz::msgs::Marker_V marker_msgs;
-
-  const float marker_width = channel_properties_.led_strip_width / msg.width();
-  const float y_start_pos = channel_properties_.led_strip_width / 2.0f - marker_width / 2.0f;
-
-  const std::string & data = msg.data();
-  const unsigned num_channels = msg.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8 ? 3
-                                                                                               : 4;
-  RGBAColor rgba;
-  for (unsigned marker_idx = 0; marker_idx < msg.width(); marker_idx++) {
-    const unsigned i = marker_idx * num_channels;
-    rgba.r = static_cast<unsigned char>(data[i]) / 255.0f;
-    rgba.g = static_cast<unsigned char>(data[i + 1]) / 255.0f;
-    rgba.b = static_cast<unsigned char>(data[i + 2]) / 255.0f;
-    rgba.a = num_channels == 4 ? static_cast<unsigned char>(data[i + 3]) / 255.0f : 1.0f;
-
-    auto marker_msg = marker_msgs.add_marker();
-    CreateMarker(marker_msg, marker_idx + first_led_marker_idx_);
-    SetMarkerColor(marker_msg, rgba);
-
-    // Set the position and size of the box
-    float marker_y_pos = static_cast<float>(marker_idx) * marker_width - y_start_pos;
-    gz::msgs::Set(
-      marker_msg->mutable_pose(),
-      gz::math::Pose3d(
-        channel_properties_.position[0], channel_properties_.position[1] + marker_y_pos,
-        channel_properties_.position[2], channel_properties_.orientation[0],
-        channel_properties_.orientation[1], channel_properties_.orientation[2]));
-    gz::msgs::Set(marker_msg->mutable_scale(), gz::math::Vector3d(0.001, marker_width, 0.015));
-  }
-
-  gz::msgs::Boolean res;
-  bool result;
-  node_->Request("/marker_array", marker_msgs, 1u, res, result);
-}
-
-RGBAColor LEDStrip::CalculateMeanRGBA(const gz::msgs::Image & msg)
-{
-  const std::string & data = msg.data();
-  const unsigned num_channels = msg.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8 ? 3
-                                                                                               : 4;
-  size_t sum_r = 0, sum_g = 0, sum_b = 0, sum_a = 0;
-
-  for (size_t i = 0; i < data.size(); i += num_channels) {
-    sum_r += static_cast<unsigned char>(data[i]);
-    sum_g += static_cast<unsigned char>(data[i + 1]);
-    sum_b += static_cast<unsigned char>(data[i + 2]);
-    sum_a += static_cast<unsigned char>(data[i + 3]);
-  }
-
-  RGBAColor rgba;
-  rgba.r = static_cast<float>(sum_r) / msg.width() / 255.0f;
-  rgba.g = static_cast<float>(sum_g) / msg.width() / 255.0f;
-  rgba.b = static_cast<float>(sum_b) / msg.width() / 255.0f;
-  rgba.a = num_channels == 4 ? static_cast<float>(sum_a) / msg.width() / 255.0f : 1;
-
-  return rgba;
-}
-
-void LEDStrip::PublishLight(const RGBAColor & rgba)
-{
-  gz::msgs::Light msg;
-  msg.set_name(channel_properties_.light_name);
-  msg.set_type(gz::msgs::Light::SPOT);
-  // msg.set_visible(false); // Available in gazebo garden
-
-  auto * diffuse = msg.mutable_diffuse();
-  diffuse->set_r(rgba.r);
-  diffuse->set_g(rgba.g);
-  diffuse->set_b(rgba.b);
-  diffuse->set_a(rgba.a);
-
-  msg.set_cast_shadows(true);
-  auto * specular = msg.mutable_specular();
-  specular->set_r(rgba.r);
-  specular->set_g(rgba.g);
-  specular->set_b(rgba.b);
-  specular->set_a(rgba.a);
-
-  msg.set_spot_inner_angle(1.0);
-  msg.set_spot_outer_angle(2.0);
-  msg.set_spot_falloff(0.4);
-
-  msg.set_attenuation_constant(1.0);
-  msg.set_attenuation_linear(1.0);
-  msg.set_attenuation_quadratic(0.5);
-
-  msg.set_intensity(1.0);
-  msg.set_range(20.0);
-
-  auto * direction = msg.mutable_direction();
-  direction->set_x(1.0);
-  direction->set_y(0.0);
-  direction->set_z(-0.5);
-
-  light_pub_->Publish(msg);
-}
-
-void LEDStrip::CreateMarker(ignition::msgs::Marker * marker, const int id)
-{
-  marker->set_ns("default");
-  marker->set_id(id);
-  marker->set_parent(channel_properties_.parent_link);
-  marker->set_action(gz::msgs::Marker::ADD_MODIFY);
-  marker->set_type(gz::msgs::Marker::BOX);
-  marker->set_visibility(gz::msgs::Marker::GUI);
-}
-
-void LEDStrip::SetMarkerColor(gz::msgs::Marker * marker, const RGBAColor & rgba)
-{
-  float r = rgba.r;
-  float g = rgba.g;
-  float b = rgba.b;
-  float a = rgba.a;
-
-  // Make default gray color
-  float max_brightness = std::max({r, g, b});
-  r = std::max(r, 0.5f - max_brightness / 2.0f);
-  g = std::max(g, 0.5f - max_brightness / 2.0f);
-  b = std::max(b, 0.5f - max_brightness / 2.0f);
-
-  marker->mutable_material()->mutable_ambient()->set_r(r);
-  marker->mutable_material()->mutable_ambient()->set_g(g);
-  marker->mutable_material()->mutable_ambient()->set_b(b);
-  marker->mutable_material()->mutable_ambient()->set_a(a);
-  marker->mutable_material()->mutable_diffuse()->set_r(r);
-  marker->mutable_material()->mutable_diffuse()->set_g(g);
-  marker->mutable_material()->mutable_diffuse()->set_b(b);
-  marker->mutable_material()->mutable_diffuse()->set_a(a);
-}
diff --git a/panther_gazebo/src/gz_led_strip_manager.cpp b/panther_gazebo/src/gz_led_strip_manager.cpp
deleted file mode 100644
index be6fa478f..000000000
--- a/panther_gazebo/src/gz_led_strip_manager.cpp
+++ /dev/null
@@ -1,68 +0,0 @@
-// Copyright 2024 Husarion sp. z o.o.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "panther_gazebo/gz_led_strip_manager.hpp"
-
-#include <cstring>
-#include <exception>
-#include <iostream>
-#include <stdexcept>
-#include <string>
-#include <vector>
-
-#include <yaml-cpp/yaml.h>
-
-#include "panther_utils/yaml_utils.hpp"
-
-LEDStripManager::LEDStripManager(const std::string & config_file)
-{
-  LoadConfig(config_file);
-  CreateLEDStrips();
-}
-
-void LEDStripManager::LoadConfig(const std::string & config_file)
-{
-  try {
-    config_ = YAML::LoadFile(config_file);
-  } catch (const std::exception & e) {
-    throw std::runtime_error("Error loading configuration from " + config_file + ": " + e.what());
-  }
-}
-
-void LEDStripManager::CreateLEDStrips()
-{
-  for (const auto & it : config_) {
-    YAML::Node channel_args = it.second;
-
-    ChannelProperties channel_prop;
-    AssignProperty(channel_args, channel_prop.frequency, "frequency");
-    AssignProperty(channel_args, channel_prop.world_name, "world_name");
-    AssignProperty(channel_args, channel_prop.parent_link, "parent_link");
-    AssignProperty(channel_args, channel_prop.position, "position");
-    AssignProperty(channel_args, channel_prop.orientation, "orientation");
-    AssignProperty(channel_args, channel_prop.led_strip_width, "led_strip_width");
-    AssignProperty(channel_args, channel_prop.topic, "topic");
-    AssignProperty(channel_args, channel_prop.light_name, "light_name");
-    AssignProperty(channel_args, channel_prop.number_of_leds, "number_of_leds");
-
-    led_strips_.emplace_back(channel_prop);
-  }
-}
-
-template <typename T>
-void LEDStripManager::AssignProperty(
-  const YAML::Node & channel_args, T & property, const std::string & key)
-{
-  property = panther_utils::GetYAMLKeyValue<YAML::Node>(channel_args, key).as<T>();
-}
diff --git a/panther_gazebo/src/led_strip.cpp b/panther_gazebo/src/led_strip.cpp
new file mode 100644
index 000000000..c7820bb1f
--- /dev/null
+++ b/panther_gazebo/src/led_strip.cpp
@@ -0,0 +1,282 @@
+// Copyright 2024 Husarion sp. z o.o.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "panther_gazebo/led_strip.hpp"
+
+#include <cstddef>
+#include <limits>
+
+#include <gz/plugin/Register.hh>
+#include <gz/sim/Model.hh>
+#include <gz/sim/components/LightCmd.hh>
+#include <gz/sim/components/Name.hh>
+
+#include <gz/msgs/color.pb.h>
+#include <gz/msgs/marker.pb.h>
+
+namespace panther_gazebo
+{
+void LEDStrip::Configure(
+  const gz::sim::Entity & entity, const std::shared_ptr<const sdf::Element> & sdf,
+  gz::sim::EntityComponentManager & ecm, gz::sim::EventManager & /*eventMgr*/)
+{
+  const auto model = gz::gazebo::Model(entity);
+  if (!model.Valid(ecm)) {
+    throw std::runtime_error(
+      "Error: Failed to initialize because [" + model.Name(ecm) +
+      "] (Entity=" + std::to_string(entity) +
+      ") is not a model. Please make sure that LEDStrip is attached to a valid model.");
+  }
+
+  ParseParameters(sdf);
+  light_cmd_ = SetupLightCmd(ecm);
+
+  node_.Subscribe(ns_ + "/" + image_topic_, &LEDStrip::ImageCallback, this);
+}
+
+void LEDStrip::PreUpdate(const gz::sim::UpdateInfo & info, gz::sim::EntityComponentManager & ecm)
+{
+  auto current_time = info.simTime;
+  auto period = std::chrono::milliseconds(static_cast<int>(1000 / frequency_));
+
+  if (new_image_available_ && current_time - last_update_time_ >= period) {
+    gz::msgs::Image image;
+    {
+      last_image_.get(image);
+      new_image_available_ = false;
+    }
+
+    VisualizeLights(ecm, image);
+
+    auto light_pose = ecm.Component<gz::sim::components::Pose>(light_entity_)->Data();
+    VisualizeMarkers(image, light_pose);
+
+    last_update_time_ = current_time;
+  }
+}
+
+void LEDStrip::ParseParameters(const std::shared_ptr<const sdf::Element> & sdf)
+{
+  if (!sdf->HasElement("light_name")) {
+    throw std::runtime_error("Error: The light_name parameter is missing.");
+  }
+  light_name_ = sdf->Get<std::string>("light_name");
+
+  if (!sdf->HasElement("topic")) {
+    throw std::runtime_error("Error: The topic parameter is missing.");
+  }
+  image_topic_ = sdf->Get<std::string>("topic");
+
+  ns_ = sdf->HasElement("namespace") ? sdf->Get<std::string>("namespace") : ns_;
+  frequency_ = sdf->HasElement("frequency") ? sdf->Get<double>("frequency") : frequency_;
+  marker_width_ = sdf->HasElement("width") ? sdf->Get<double>("width") : marker_width_;
+  marker_height_ = sdf->HasElement("height") ? sdf->Get<double>("height") : marker_height_;
+}
+
+gz::msgs::Light LEDStrip::SetupLightCmd(gz::sim::EntityComponentManager & ecm)
+{
+  gz::msgs::Light light_cmd;
+
+  ecm.Each<gz::sim::components::Name, gz::sim::components::Light>(
+    [&](
+      const gz::sim::Entity & entity, const gz::sim::components::Name * name,
+      const gz::sim::components::Light * light_component) -> bool {
+      if (name->Data() != light_name_) {
+        return true;  // Continue searching
+      }
+      light_entity_ = entity;
+      igndbg << "Light entity found: " << light_entity_ << std::endl;
+      light_cmd = CreateLightMsgFromSdf(light_component->Data());
+      return false;  // Stop searching
+    });
+
+  if (light_entity_ == gz::sim::kNullEntity) {
+    throw std::runtime_error("Error: Light entity not found.");
+  }
+  return light_cmd;
+}
+
+gz::msgs::Light LEDStrip::CreateLightMsgFromSdf(const sdf::Light & light_sdf)
+{
+  gz::msgs::Light light_cmd;
+
+  light_cmd.set_name(light_sdf.Name());
+  light_cmd.set_range(light_sdf.AttenuationRange());
+  light_cmd.set_cast_shadows(light_sdf.CastShadows());
+  light_cmd.set_spot_inner_angle(light_sdf.SpotInnerAngle().Radian());
+  light_cmd.set_spot_outer_angle(light_sdf.SpotOuterAngle().Radian());
+  light_cmd.set_spot_falloff(light_sdf.SpotFalloff());
+  light_cmd.set_attenuation_constant(light_sdf.ConstantAttenuationFactor());
+  light_cmd.set_attenuation_linear(light_sdf.LinearAttenuationFactor());
+  light_cmd.set_attenuation_quadratic(light_sdf.QuadraticAttenuationFactor());
+  light_cmd.set_intensity(light_sdf.Intensity());
+
+  gz::msgs::Set(light_cmd.mutable_diffuse(), light_sdf.Diffuse());
+  gz::msgs::Set(light_cmd.mutable_specular(), light_sdf.Specular());
+  gz::msgs::Set(light_cmd.mutable_direction(), light_sdf.Direction());
+
+  // Set the light type
+  switch (light_sdf.Type()) {
+    case sdf::LightType::POINT:
+      light_cmd.set_type(gz::msgs::Light::POINT);
+      break;
+    case sdf::LightType::SPOT:
+      light_cmd.set_type(gz::msgs::Light::SPOT);
+      break;
+    case sdf::LightType::DIRECTIONAL:
+      light_cmd.set_type(gz::msgs::Light::DIRECTIONAL);
+      break;
+    default:
+      light_cmd.set_type(gz::msgs::Light::POINT);
+      break;
+  }
+
+  return light_cmd;
+}
+
+void LEDStrip::ImageCallback(const gz::msgs::Image & msg)
+{
+  if (!IsEncodingValid(msg)) {
+    ignerr << "Error: Incorrect image encoding." << std::endl;
+    return;
+  }
+
+  last_image_.set(msg);
+  new_image_available_ = true;
+}
+
+bool LEDStrip::IsEncodingValid(const gz::msgs::Image & msg)
+{
+  return msg.pixel_format_type() == gz::msgs::PixelFormatType::RGBA_INT8 ||
+         msg.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8;
+}
+
+gz::math::Color LEDStrip::CalculateMeanColor(const gz::msgs::Image & msg)
+{
+  size_t sum_r = 0, sum_g = 0, sum_b = 0, sum_a = 0;
+  size_t pixel_count = msg.width() * msg.height();
+
+  const std::string & data = msg.data();
+  bool is_rgba = (msg.pixel_format_type() == gz::msgs::PixelFormatType::RGBA_INT8);
+  int step = is_rgba ? 4 : 3;
+
+  for (size_t i = 0; i < pixel_count * step; i += step) {
+    sum_r += static_cast<uint8_t>(data[i]);
+    sum_g += static_cast<uint8_t>(data[i + 1]);
+    sum_b += static_cast<uint8_t>(data[i + 2]);
+    if (is_rgba) {
+      sum_a += static_cast<uint8_t>(data[i + 3]);
+    }
+  }
+
+  const float max_value = std::numeric_limits<uint8_t>::max();
+  const float norm_factor = max_value * pixel_count;
+
+  float norm_mean_r = sum_r / norm_factor;
+  float norm_mean_g = sum_g / norm_factor;
+  float norm_mean_b = sum_b / norm_factor;
+  float norm_mean_a = is_rgba ? sum_a / norm_factor : 1.0f;
+
+  auto mean_color = gz::math::Color(norm_mean_r, norm_mean_g, norm_mean_b, norm_mean_a);
+
+  return mean_color;
+}
+
+void LEDStrip::VisualizeLights(gz::sim::EntityComponentManager & ecm, const gz::msgs::Image & image)
+{
+  gz::math::Color mean_color = CalculateMeanColor(image);
+
+  gz::msgs::Set(light_cmd_.mutable_diffuse(), mean_color);
+  gz::msgs::Set(light_cmd_.mutable_specular(), mean_color);
+
+  auto light_on = light_cmd_.mutable_header()->add_data();
+  light_on->set_key("isLightOn");
+  light_on->add_value()->assign("1");
+
+  auto visualize = light_cmd_.mutable_header()->add_data();
+  visualize->set_key("visualizeVisual");
+  visualize->add_value()->assign("0");
+
+  ecm.SetComponentData<gz::sim::components::LightCmd>(light_entity_, light_cmd_);
+
+  ecm.SetChanged(
+    light_entity_, gz::sim::components::LightCmd::typeId, gz::sim::ComponentState::PeriodicChange);
+}
+
+void LEDStrip::VisualizeMarkers(const gz::msgs::Image & image, const gz::math::Pose3d & light_pose)
+{
+  const std::string & data = image.data();
+
+  double step_width = marker_width_ / image.width();
+  double step_height = marker_height_ / image.height();
+
+  const float max_value = std::numeric_limits<uint8_t>::max();
+  bool is_rgba = (image.pixel_format_type() == gz::msgs::PixelFormatType::RGBA_INT8);
+  int step = is_rgba ? 4 : 3;
+
+  const auto pixel_size = gz::math::Vector3d(0.001, step_width, step_height);
+
+  for (size_t y = 0; y < image.height(); ++y) {
+    for (size_t x = 0; x < image.width(); ++x) {
+      size_t idx = (y * image.width() + x) * step;
+
+      float r = static_cast<uint8_t>(data[idx]) / max_value;
+      float g = static_cast<uint8_t>(data[idx + 1]) / max_value;
+      float b = static_cast<uint8_t>(data[idx + 2]) / max_value;
+      float a = is_rgba ? static_cast<uint8_t>(data[idx + 3]) / max_value : 1.0f;
+      auto pixel_color = gz::math::Color(r, g, b, a);
+
+      double x_pos = light_pose.Pos().X();
+      double y_pos = light_pose.Pos().Y() + x * step_width - marker_width_ / 2.0 + step_width / 2.0;
+      double z_pos = light_pose.Pos().Z() + y * step_height;
+      double roll = light_pose.Rot().Roll();
+      double pitch = light_pose.Rot().Pitch();
+      double yaw = light_pose.Rot().Yaw();
+      auto pose = gz::math::Pose3d(x_pos, y_pos, z_pos, roll, pitch, yaw);
+
+      CreateMarker(idx, pose, pixel_color, pixel_size);
+    }
+  }
+}
+
+void LEDStrip::CreateMarker(
+  const uint id, const gz::math::Pose3d pose, const gz::math::Color & color,
+  const gz::math::Vector3d size)
+{
+  gz::msgs::Marker marker_msg;
+  marker_msg.set_action(gz::msgs::Marker::ADD_MODIFY);
+  marker_msg.set_ns(ns_ + light_name_);
+  marker_msg.set_id(id + 1);  // Markers with IDs 0 cannot be overwritten
+  if (ns_.empty()) {
+    marker_msg.set_parent("panther");
+  } else {
+    marker_msg.set_parent(ns_);
+  }
+  marker_msg.set_type(gz::msgs::Marker::BOX);
+
+  gz::msgs::Set(marker_msg.mutable_pose(), pose);
+  gz::msgs::Set(marker_msg.mutable_scale(), size);
+
+  gz::msgs::Set(marker_msg.mutable_material()->mutable_ambient(), color);
+  gz::msgs::Set(marker_msg.mutable_material()->mutable_diffuse(), color);
+
+  // Using Request to ensure markers are visible
+  node_.Request("/marker", marker_msg);
+}
+
+}  // namespace panther_gazebo
+
+IGNITION_ADD_PLUGIN(
+  panther_gazebo::LEDStrip, gz::sim::System, panther_gazebo::LEDStrip::ISystemConfigure,
+  panther_gazebo::LEDStrip::ISystemPreUpdate)
diff --git a/panther_gazebo/src/main.cpp b/panther_gazebo/src/main.cpp
deleted file mode 100644
index 73b5dbc2d..000000000
--- a/panther_gazebo/src/main.cpp
+++ /dev/null
@@ -1,48 +0,0 @@
-// Copyright 2024 Husarion sp. z o.o.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <cstring>
-#include <exception>
-#include <iostream>
-
-#include "panther_gazebo/gz_led_strip_manager.hpp"
-
-int main(int argc, char ** argv)
-{
-  const char * configFilePath = nullptr;
-
-  // Check for the "--config-file" argument in the command-line arguments
-  for (int i = 1; i < argc; ++i) {  // Start from 1 to skip program name
-    if (std::strcmp(argv[i], "--config-file") == 0 && i + 1 < argc) {
-      configFilePath = argv[++i];
-      break;
-    }
-  }
-
-  // Check if the config file path was provided
-  if (configFilePath == nullptr) {
-    std::cerr << "Usage: " << argv[0] << " --config-file <config.yaml>" << std::endl;
-    return -1;
-  }
-
-  try {
-    LEDStripManager manager(configFilePath);
-    gz::transport::waitForShutdown();
-  } catch (const std::exception & e) {
-    std::cerr << "Error: " << e.what() << std::endl;
-    return -1;
-  }
-
-  return 0;
-}