From a3a3656bc9c582cd836fae5640a357bc9bf6ce11 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Mon, 19 Aug 2024 10:44:28 +0000
Subject: [PATCH 001/100] driver state update

---
 .../battery/roboteq_battery.hpp               |  1 +
 .../src/battery/roboteq_battery.cpp           | 24 ++++-
 .../test/battery/test_adc_battery.cpp         |  2 -
 .../test/battery/test_roboteq_battery.cpp     | 19 ++--
 .../test/test_battery_driver_node_roboteq.cpp | 20 ++--
 .../panther_system_ros_interface.hpp          |  7 ++
 .../panther_system_ros_interface.cpp          | 99 ++++++++++++++-----
 .../panther_system/test_panther_system.cpp    | 26 +++--
 .../test_panther_system_ros_interface.cpp     | 61 +++++++-----
 .../panther_manager/safety_manager_node.hpp   |  6 +-
 panther_manager/src/safety_manager_node.cpp   | 30 ++++--
 .../test/test_safety_behavior_tree.cpp        |  2 -
 .../test/test_safety_manager_node.cpp         |  6 +-
 13 files changed, 207 insertions(+), 96 deletions(-)

diff --git a/panther_battery/include/panther_battery/battery/roboteq_battery.hpp b/panther_battery/include/panther_battery/battery/roboteq_battery.hpp
index a0019fc5..540c5fc3 100644
--- a/panther_battery/include/panther_battery/battery/roboteq_battery.hpp
+++ b/panther_battery/include/panther_battery/battery/roboteq_battery.hpp
@@ -62,6 +62,7 @@ class RoboteqBattery : public Battery
   void UpdateBatteryStateRaw();
   void UpdateChargingStatus(const rclcpp::Time & header_stamp);
   std::uint8_t GetBatteryHealth(const float voltage);
+  bool MotorControllerHeartbeatTimeout();
 
   std::function<DriverStateMsg::SharedPtr()> GetDriverState;
 
diff --git a/panther_battery/src/battery/roboteq_battery.cpp b/panther_battery/src/battery/roboteq_battery.cpp
index 6653208e..424a06eb 100644
--- a/panther_battery/src/battery/roboteq_battery.cpp
+++ b/panther_battery/src/battery/roboteq_battery.cpp
@@ -46,8 +46,17 @@ void RoboteqBattery::Update(const rclcpp::Time & header_stamp, const bool /* cha
   driver_state_ = GetDriverState();
   ValidateDriverStateMsg(header_stamp);
 
-  voltage_raw_ = (driver_state_->front.voltage + driver_state_->rear.voltage) / 2.0f;
-  current_raw_ = driver_state_->front.current + driver_state_->rear.current;
+  float voltage = 0.0f;
+  float current = 0.0f;
+  std::for_each(
+    driver_state_->motor_controllers.begin(), driver_state_->motor_controllers.end(),
+    [&voltage, &current](const panther_msgs::msg::MotorController & driver) {
+      voltage += driver.state.voltage;
+      current += driver.state.current;
+    });
+
+  voltage_raw_ = voltage / driver_state_->motor_controllers.size();
+  current_raw_ = current;
   voltage_ma_->Roll(voltage_raw_);
   current_ma_->Roll(current_raw_);
 
@@ -74,7 +83,7 @@ void RoboteqBattery::ValidateDriverStateMsg(const rclcpp::Time & header_stamp)
     throw std::runtime_error("Driver state message timeout.");
   }
 
-  if (driver_state_->front.heartbeat_timeout || driver_state_->rear.heartbeat_timeout) {
+  if (MotorControllerHeartbeatTimeout()) {
     throw std::runtime_error("Motor controller heartbeat timeout error.");
   }
 }
@@ -141,4 +150,13 @@ std::uint8_t RoboteqBattery::GetBatteryHealth(const float voltage)
   }
 }
 
+bool RoboteqBattery::MotorControllerHeartbeatTimeout()
+{
+  return std::any_of(
+    driver_state_->motor_controllers.begin(), driver_state_->motor_controllers.end(),
+    [](const panther_msgs::msg::MotorController & driver) {
+      return driver.state.heartbeat_timeout;
+    });
+}
+
 }  // namespace panther_battery
diff --git a/panther_battery/test/battery/test_adc_battery.cpp b/panther_battery/test/battery/test_adc_battery.cpp
index 5dcbeaf2..986f7cdb 100644
--- a/panther_battery/test/battery/test_adc_battery.cpp
+++ b/panther_battery/test/battery/test_adc_battery.cpp
@@ -157,8 +157,6 @@ TEST_F(TestADCBattery, BatteryMsgValues)
   const auto voltage_factor = 25.04255;
   const auto current_factor = 20.0;
   const auto charge_factor = 2.5;
-  const auto V_bat_full = 41.4;
-  const auto V_bat_min = 32.0;
 
   const float voltage_raw_1 = 1.5;
   const float current_raw_1 = 0.01;
diff --git a/panther_battery/test/battery/test_roboteq_battery.cpp b/panther_battery/test/battery/test_roboteq_battery.cpp
index da73444e..4d15f541 100644
--- a/panther_battery/test/battery/test_roboteq_battery.cpp
+++ b/panther_battery/test/battery/test_roboteq_battery.cpp
@@ -80,15 +80,17 @@ void TestRoboteqBattery::UpdateBattery(const float voltage, const float current)
 {
   if (!driver_state_) {
     driver_state_ = std::make_shared<DriverStateMsg>();
+    driver_state_->motor_controllers.push_back(panther_msgs::msg::MotorController());
+    driver_state_->motor_controllers.push_back(panther_msgs::msg::MotorController());
   }
 
   auto stamp = rclcpp::Time(0, 0, RCL_ROS_TIME);
 
   driver_state_->header.stamp = stamp;
-  driver_state_->front.voltage = voltage;
-  driver_state_->rear.voltage = voltage;
-  driver_state_->front.current = current;
-  driver_state_->rear.current = current;
+  driver_state_->motor_controllers.at(0).state.voltage = voltage;
+  driver_state_->motor_controllers.at(1).state.voltage = voltage;
+  driver_state_->motor_controllers.at(0).state.current = current;
+  driver_state_->motor_controllers.at(1).state.current = current;
 
   battery_->Update(stamp, false);
   battery_state_ = battery_->GetBatteryMsg();
@@ -155,9 +157,6 @@ TEST_F(TestRoboteqBattery, BatteryMsgUnknown)
 
 TEST_F(TestRoboteqBattery, BatteryMsgValues)
 {
-  const float V_bat_full = 41.4;
-  const float V_bat_min = 32.0;
-
   const float voltage_1 = 35.0;
   const float current_1 = 0.1;
   UpdateBattery(voltage_1, current_1);
@@ -243,10 +242,10 @@ TEST_F(TestRoboteqBattery, ValidateDriverStateMsg)
   EXPECT_NO_THROW(battery_->ValidateDriverStateMsg(stamp));
 
   // Check heartbeat timeout error throw
-  driver_state_->front.heartbeat_timeout = true;
+  driver_state_->motor_controllers.at(0).state.heartbeat_timeout = true;
   EXPECT_THROW(battery_->ValidateDriverStateMsg(stamp), std::runtime_error);
-  driver_state_->front.heartbeat_timeout = false;
-  driver_state_->rear.heartbeat_timeout = true;
+  driver_state_->motor_controllers.at(0).state.heartbeat_timeout = false;
+  driver_state_->motor_controllers.at(1).state.heartbeat_timeout = true;
   EXPECT_THROW(battery_->ValidateDriverStateMsg(stamp), std::runtime_error);
 }
 
diff --git a/panther_battery/test/test_battery_driver_node_roboteq.cpp b/panther_battery/test/test_battery_driver_node_roboteq.cpp
index c4cf8f9c..757faaa0 100644
--- a/panther_battery/test/test_battery_driver_node_roboteq.cpp
+++ b/panther_battery/test/test_battery_driver_node_roboteq.cpp
@@ -44,12 +44,14 @@ TEST_F(TestBatteryNodeRoboteq, BatteryValues)
   EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_STATUS_UNKNOWN, battery_state_->power_supply_status);
   EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_HEALTH_UNKNOWN, battery_state_->power_supply_health);
 
+  panther_msgs::msg::MotorController motor_controller;
+  motor_controller.state.voltage = 35.0f;
+  motor_controller.state.current = 0.1f;
+
   DriverStateMsg driver_state;
   driver_state.header.stamp = battery_driver_node_->get_clock()->now();
-  driver_state.front.voltage = 35.0f;
-  driver_state.rear.voltage = 35.0f;
-  driver_state.front.current = 0.1f;
-  driver_state.rear.current = 0.1f;
+  driver_state.motor_controllers.push_back(motor_controller);
+  driver_state.motor_controllers.push_back(motor_controller);
   driver_state_pub_->publish(driver_state);
 
   ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
@@ -82,12 +84,14 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout)
   EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_HEALTH_UNKNOWN, battery_state_->power_supply_health);
 
   // Publish some values
+  panther_msgs::msg::MotorController motor_controller;
+  motor_controller.state.voltage = 35.0f;
+  motor_controller.state.current = 0.1f;
+
   DriverStateMsg driver_state;
   driver_state.header.stamp = battery_driver_node_->get_clock()->now();
-  driver_state.front.voltage = 35.0f;
-  driver_state.rear.voltage = 35.0f;
-  driver_state.front.current = 0.1f;
-  driver_state.rear.current = 0.1f;
+  driver_state.motor_controllers.push_back(motor_controller);
+  driver_state.motor_controllers.push_back(motor_controller);
   driver_state_pub_->publish(driver_state);
 
   ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp
index e27a0b36..a117737f 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp
@@ -32,6 +32,7 @@
 
 #include "panther_msgs/msg/driver_state.hpp"
 #include "panther_msgs/msg/io_state.hpp"
+#include "panther_msgs/msg/motor_controller.hpp"
 
 #include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp"
 #include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp"
@@ -44,6 +45,7 @@ namespace panther_hardware_interfaces
 using BoolMsg = std_msgs::msg::Bool;
 using DriverStateMsg = panther_msgs::msg::DriverState;
 using IOStateMsg = panther_msgs::msg::IOState;
+using MotorControllerMsg = panther_msgs::msg::MotorController;
 using SetBoolSrv = std_srvs::srv::SetBool;
 using TriggerSrv = std_srvs::srv::Trigger;
 
@@ -269,6 +271,11 @@ class PantherSystemRosInterface
   rclcpp::CallbackGroup::SharedPtr GetOrCreateNodeCallbackGroup(
     const unsigned group_id, rclcpp::CallbackGroupType callback_group_type);
 
+  void InitializeDriverStateMsg();
+
+  MotorControllerMsg & GetMotorControllerByName(
+    DriverStateMsg & driver_state, const std::string & name);
+
   rclcpp::Node::SharedPtr node_;
   std::unordered_map<unsigned, rclcpp::CallbackGroup::SharedPtr> callback_groups_;
   rclcpp::executors::MultiThreadedExecutor::UniquePtr executor_;
diff --git a/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp b/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp
index 7bab66df..6766d85f 100644
--- a/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp
+++ b/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp
@@ -86,6 +86,8 @@ PantherSystemRosInterface::PantherSystemRosInterface(
   realtime_driver_state_publisher_ =
     std::make_unique<realtime_tools::RealtimePublisher<DriverStateMsg>>(driver_state_publisher_);
 
+  InitializeDriverStateMsg();
+
   io_state_publisher_ = node_->create_publisher<IOStateMsg>(
     "~/io_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
   realtime_io_state_publisher_ =
@@ -129,56 +131,73 @@ void PantherSystemRosInterface::UpdateMsgErrorFlags(
   const RoboteqData & front, const RoboteqData & rear)
 {
   auto & driver_state = realtime_driver_state_publisher_->msg_;
+  auto & front_motor_controller = GetMotorControllerByName(
+    driver_state, MotorControllerMsg::NAME_FRONT);
+  auto & rear_motor_controller = GetMotorControllerByName(
+    driver_state, MotorControllerMsg::NAME_REAR);
 
   driver_state.header.stamp = node_->get_clock()->now();
 
-  driver_state.front.fault_flag = front.GetFaultFlag().GetMessage();
-  driver_state.front.script_flag = front.GetScriptFlag().GetMessage();
-  driver_state.front.left_motor_runtime_error = front.GetLeftRuntimeError().GetMessage();
-  driver_state.front.right_motor_runtime_error = front.GetRightRuntimeError().GetMessage();
+  front_motor_controller.state.fault_flag = front.GetFaultFlag().GetMessage();
+  front_motor_controller.state.script_flag = front.GetScriptFlag().GetMessage();
+  front_motor_controller.state.left_motor_runtime_error = front.GetLeftRuntimeError().GetMessage();
+  front_motor_controller.state.right_motor_runtime_error =
+    front.GetRightRuntimeError().GetMessage();
 
-  driver_state.rear.fault_flag = rear.GetFaultFlag().GetMessage();
-  driver_state.rear.script_flag = rear.GetScriptFlag().GetMessage();
-  driver_state.rear.left_motor_runtime_error = rear.GetLeftRuntimeError().GetMessage();
-  driver_state.rear.right_motor_runtime_error = rear.GetRightRuntimeError().GetMessage();
+  rear_motor_controller.state.fault_flag = rear.GetFaultFlag().GetMessage();
+  rear_motor_controller.state.script_flag = rear.GetScriptFlag().GetMessage();
+  rear_motor_controller.state.left_motor_runtime_error = rear.GetLeftRuntimeError().GetMessage();
+  rear_motor_controller.state.right_motor_runtime_error = rear.GetRightRuntimeError().GetMessage();
 }
 
 void PantherSystemRosInterface::UpdateMsgDriversStates(
   const DriverState & front, const DriverState & rear)
 {
   auto & driver_state = realtime_driver_state_publisher_->msg_;
-
-  driver_state.front.voltage = front.GetVoltage();
-  driver_state.front.current = front.GetCurrent();
-  driver_state.front.temperature = front.GetTemperature();
-  driver_state.front.heatsink_temperature = front.GetHeatsinkTemperature();
-
-  driver_state.rear.voltage = rear.GetVoltage();
-  driver_state.rear.current = rear.GetCurrent();
-  driver_state.rear.temperature = rear.GetTemperature();
-  driver_state.rear.heatsink_temperature = rear.GetHeatsinkTemperature();
+  auto & front_motor_controller = GetMotorControllerByName(
+    driver_state, MotorControllerMsg::NAME_FRONT);
+  auto & rear_motor_controller = GetMotorControllerByName(
+    driver_state, MotorControllerMsg::NAME_REAR);
+
+  front_motor_controller.state.voltage = front.GetVoltage();
+  front_motor_controller.state.current = front.GetCurrent();
+  front_motor_controller.state.temperature = front.GetTemperature();
+  front_motor_controller.state.heatsink_temperature = front.GetHeatsinkTemperature();
+
+  rear_motor_controller.state.voltage = rear.GetVoltage();
+  rear_motor_controller.state.current = rear.GetCurrent();
+  rear_motor_controller.state.temperature = rear.GetTemperature();
+  rear_motor_controller.state.heatsink_temperature = rear.GetHeatsinkTemperature();
 }
 
 void PantherSystemRosInterface::UpdateMsgErrors(const CANErrors & can_errors)
 {
   auto & driver_state = realtime_driver_state_publisher_->msg_;
+  auto & front_motor_controller = GetMotorControllerByName(
+    driver_state, MotorControllerMsg::NAME_FRONT);
+  auto & rear_motor_controller = GetMotorControllerByName(
+    driver_state, MotorControllerMsg::NAME_REAR);
 
   driver_state.error = can_errors.error;
   driver_state.write_pdo_cmds_error = can_errors.write_pdo_cmds_error;
   driver_state.read_pdo_motor_states_error = can_errors.read_pdo_motor_states_error;
   driver_state.read_pdo_driver_state_error = can_errors.read_pdo_driver_state_error;
 
-  driver_state.front.motor_states_data_timed_out = can_errors.front_motor_states_data_timed_out;
-  driver_state.rear.motor_states_data_timed_out = can_errors.rear_motor_states_data_timed_out;
+  front_motor_controller.state.motor_states_data_timed_out =
+    can_errors.front_motor_states_data_timed_out;
+  rear_motor_controller.state.motor_states_data_timed_out =
+    can_errors.rear_motor_states_data_timed_out;
 
-  driver_state.front.driver_state_data_timed_out = can_errors.front_driver_state_data_timed_out;
-  driver_state.rear.driver_state_data_timed_out = can_errors.rear_driver_state_data_timed_out;
+  front_motor_controller.state.driver_state_data_timed_out =
+    can_errors.front_driver_state_data_timed_out;
+  rear_motor_controller.state.driver_state_data_timed_out =
+    can_errors.rear_driver_state_data_timed_out;
 
-  driver_state.front.can_error = can_errors.front_can_error;
-  driver_state.rear.can_error = can_errors.rear_can_error;
+  front_motor_controller.state.can_error = can_errors.front_can_error;
+  rear_motor_controller.state.can_error = can_errors.rear_can_error;
 
-  driver_state.front.heartbeat_timeout = can_errors.front_heartbeat_timeout;
-  driver_state.rear.heartbeat_timeout = can_errors.rear_heartbeat_timeout;
+  front_motor_controller.state.heartbeat_timeout = can_errors.front_heartbeat_timeout;
+  rear_motor_controller.state.heartbeat_timeout = can_errors.rear_heartbeat_timeout;
 }
 
 void PantherSystemRosInterface::PublishEStopStateMsg(const bool e_stop)
@@ -290,4 +309,32 @@ rclcpp::CallbackGroup::SharedPtr PantherSystemRosInterface::GetOrCreateNodeCallb
   return callback_group;
 }
 
+void PantherSystemRosInterface::InitializeDriverStateMsg()
+{
+  MotorControllerMsg front_motor_controller;
+  MotorControllerMsg rear_motor_controller;
+  front_motor_controller.name = MotorControllerMsg::NAME_FRONT;
+  rear_motor_controller.name = MotorControllerMsg::NAME_REAR;
+
+  auto & driver_state = realtime_driver_state_publisher_->msg_;
+  driver_state.motor_controllers.push_back(front_motor_controller);
+  driver_state.motor_controllers.push_back(rear_motor_controller);
+}
+
+MotorControllerMsg & PantherSystemRosInterface::GetMotorControllerByName(
+  DriverStateMsg & driver_state, const std::string & name)
+{
+  auto & motor_controllers = driver_state.motor_controllers;
+
+  auto it = std::find_if(
+    motor_controllers.begin(), motor_controllers.end(),
+    [&name](const MotorControllerMsg & msg) { return msg.name == name; });
+
+  if (it == motor_controllers.end()) {
+    throw std::runtime_error("Motor controller with name '" + name + "' not found.");
+  }
+
+  return *it;
+}
+
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp b/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp
index 6ea1cf17..c4d7d50b 100644
--- a/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp
+++ b/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp
@@ -451,20 +451,28 @@ TEST_F(TestPantherSystem, ReadOtherRoboteqParamsPantherSystem)
 
   ASSERT_TRUE(state_msg);
 
-  ASSERT_EQ(static_cast<std::int16_t>(state_msg->front.temperature), f_temp);
-  ASSERT_EQ(static_cast<std::int16_t>(state_msg->rear.temperature), r_temp);
+  ASSERT_EQ(
+    static_cast<std::int16_t>(state_msg->motor_controllers.at(0).state.temperature), f_temp);
+  ASSERT_EQ(
+    static_cast<std::int16_t>(state_msg->motor_controllers.at(1).state.temperature), r_temp);
 
-  ASSERT_EQ(static_cast<std::int16_t>(state_msg->front.heatsink_temperature), f_heatsink_temp);
-  ASSERT_EQ(static_cast<std::int16_t>(state_msg->rear.heatsink_temperature), r_heatsink_temp);
+  ASSERT_EQ(
+    static_cast<std::int16_t>(state_msg->motor_controllers.at(0).state.heatsink_temperature),
+    f_heatsink_temp);
+  ASSERT_EQ(
+    static_cast<std::int16_t>(state_msg->motor_controllers.at(1).state.heatsink_temperature),
+    r_heatsink_temp);
 
-  ASSERT_EQ(static_cast<std::uint16_t>(state_msg->front.voltage * 10.0), f_volt);
-  ASSERT_EQ(static_cast<std::uint16_t>(state_msg->rear.voltage * 10.0), r_volt);
+  ASSERT_EQ(
+    static_cast<std::uint16_t>(state_msg->motor_controllers.at(0).state.voltage * 10.0), f_volt);
+  ASSERT_EQ(
+    static_cast<std::uint16_t>(state_msg->motor_controllers.at(1).state.voltage * 10.0), r_volt);
 
   ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->front.current * 10.0),
+    static_cast<std::int16_t>(state_msg->motor_controllers.at(0).state.current * 10.0),
     (f_battery_current_1 + f_battery_current_2));
   ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->rear.current * 10.0),
+    static_cast<std::int16_t>(state_msg->motor_controllers.at(1).state.current * 10.0),
     (r_battery_current_1 + r_battery_current_2));
 
   pth_test_.ShutdownPantherSystem();
@@ -497,7 +505,7 @@ TEST_F(TestPantherSystem, EncoderDisconnectedPantherSystem)
   pth_test_.GetResourceManager()->read(TIME, PERIOD);
 
   ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5)));
-  ASSERT_TRUE(state_msg->front.script_flag.encoder_disconnected);
+  ASSERT_TRUE(state_msg->motor_controllers.at(0).state.script_flag.encoder_disconnected);
 
   // writing should be blocked - error
 
diff --git a/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp b/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
index 7389a322..75fbd40f 100644
--- a/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
+++ b/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
@@ -159,15 +159,18 @@ TEST_F(TestPantherSystemRosInterface, ErrorFlags)
   ASSERT_TRUE(
     panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5)));
 
-  EXPECT_TRUE(driver_state_msg_->front.fault_flag.overheat);
-  EXPECT_TRUE(driver_state_msg_->front.script_flag.encoder_disconnected);
-  EXPECT_TRUE(driver_state_msg_->front.left_motor_runtime_error.loop_error);
-  EXPECT_TRUE(driver_state_msg_->front.right_motor_runtime_error.safety_stop_active);
-
-  EXPECT_TRUE(driver_state_msg_->rear.fault_flag.overvoltage);
-  EXPECT_TRUE(driver_state_msg_->rear.script_flag.loop_error);
-  EXPECT_TRUE(driver_state_msg_->rear.left_motor_runtime_error.forward_limit_triggered);
-  EXPECT_TRUE(driver_state_msg_->rear.right_motor_runtime_error.reverse_limit_triggered);
+  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(0).state.fault_flag.overheat);
+  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(0).state.script_flag.encoder_disconnected);
+  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(0).state.left_motor_runtime_error.loop_error);
+  EXPECT_TRUE(
+    driver_state_msg_->motor_controllers.at(0).state.right_motor_runtime_error.safety_stop_active);
+
+  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(1).state.fault_flag.overvoltage);
+  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(1).state.script_flag.loop_error);
+  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(1)
+                .state.left_motor_runtime_error.forward_limit_triggered);
+  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(1)
+                .state.right_motor_runtime_error.reverse_limit_triggered);
 }
 
 TEST_F(TestPantherSystemRosInterface, DriversStates)
@@ -204,22 +207,34 @@ TEST_F(TestPantherSystemRosInterface, DriversStates)
   ASSERT_TRUE(
     panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5)));
 
-  EXPECT_FLOAT_EQ(static_cast<std::int16_t>(driver_state_msg_->front.temperature), f_temp);
-  EXPECT_FLOAT_EQ(static_cast<std::int16_t>(driver_state_msg_->rear.temperature), r_temp);
+  EXPECT_FLOAT_EQ(
+    static_cast<std::int16_t>(driver_state_msg_->motor_controllers.at(0).state.temperature),
+    f_temp);
+  EXPECT_FLOAT_EQ(
+    static_cast<std::int16_t>(driver_state_msg_->motor_controllers.at(1).state.temperature),
+    r_temp);
 
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->front.heatsink_temperature), f_heatsink_temp);
+    static_cast<std::int16_t>(
+      driver_state_msg_->motor_controllers.at(0).state.heatsink_temperature),
+    f_heatsink_temp);
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->rear.heatsink_temperature), r_heatsink_temp);
+    static_cast<std::int16_t>(
+      driver_state_msg_->motor_controllers.at(1).state.heatsink_temperature),
+    r_heatsink_temp);
 
-  EXPECT_FLOAT_EQ(static_cast<std::uint16_t>(driver_state_msg_->front.voltage * 10.0), f_volt);
-  EXPECT_FLOAT_EQ(static_cast<std::uint16_t>(driver_state_msg_->rear.voltage * 10.0), r_volt);
+  EXPECT_FLOAT_EQ(
+    static_cast<std::uint16_t>(driver_state_msg_->motor_controllers.at(0).state.voltage * 10.0),
+    f_volt);
+  EXPECT_FLOAT_EQ(
+    static_cast<std::uint16_t>(driver_state_msg_->motor_controllers.at(1).state.voltage * 10.0),
+    r_volt);
 
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->front.current * 10.0),
+    static_cast<std::int16_t>(driver_state_msg_->motor_controllers.at(0).state.current * 10.0),
     (f_battery_current_1 + f_battery_current_2));
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->rear.current * 10.0),
+    static_cast<std::int16_t>(driver_state_msg_->motor_controllers.at(1).state.current * 10.0),
     (r_battery_current_1 + r_battery_current_2));
 }
 
@@ -254,14 +269,14 @@ TEST_F(TestPantherSystemRosInterface, Errors)
   EXPECT_FALSE(driver_state_msg_->read_pdo_motor_states_error);
   EXPECT_FALSE(driver_state_msg_->read_pdo_driver_state_error);
 
-  EXPECT_TRUE(driver_state_msg_->front.motor_states_data_timed_out);
-  EXPECT_FALSE(driver_state_msg_->rear.motor_states_data_timed_out);
+  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(0).state.motor_states_data_timed_out);
+  EXPECT_FALSE(driver_state_msg_->motor_controllers.at(1).state.motor_states_data_timed_out);
 
-  EXPECT_FALSE(driver_state_msg_->front.driver_state_data_timed_out);
-  EXPECT_TRUE(driver_state_msg_->rear.driver_state_data_timed_out);
+  EXPECT_FALSE(driver_state_msg_->motor_controllers.at(0).state.driver_state_data_timed_out);
+  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(1).state.driver_state_data_timed_out);
 
-  EXPECT_FALSE(driver_state_msg_->front.can_error);
-  EXPECT_TRUE(driver_state_msg_->rear.can_error);
+  EXPECT_FALSE(driver_state_msg_->motor_controllers.at(0).state.can_error);
+  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(1).state.can_error);
 }
 
 int main(int argc, char ** argv)
diff --git a/panther_manager/include/panther_manager/safety_manager_node.hpp b/panther_manager/include/panther_manager/safety_manager_node.hpp
index fcfcf683..59b97f1f 100644
--- a/panther_manager/include/panther_manager/safety_manager_node.hpp
+++ b/panther_manager/include/panther_manager/safety_manager_node.hpp
@@ -15,6 +15,7 @@
 #ifndef PANTHER_MANAGER_SAFETY_MANAGER_NODE_HPP_
 #define PANTHER_MANAGER_SAFETY_MANAGER_NODE_HPP_
 
+#include <map>
 #include <memory>
 #include <string>
 
@@ -85,6 +86,7 @@ class SafetyManagerNode : public rclcpp::Node
   static constexpr float kCriticalBatteryTemp = 55.0;
   static constexpr float kFatalBatteryTemp = 62.0;
 
+  int driver_temp_window_len_;
   float update_charging_anim_step_;
 
   rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_sub_;
@@ -96,8 +98,8 @@ class SafetyManagerNode : public rclcpp::Node
 
   std::unique_ptr<panther_utils::MovingAverage<double>> battery_temp_ma_;
   std::unique_ptr<panther_utils::MovingAverage<double>> cpu_temp_ma_;
-  std::unique_ptr<panther_utils::MovingAverage<double>> front_driver_temp_ma_;
-  std::unique_ptr<panther_utils::MovingAverage<double>> rear_driver_temp_ma_;
+
+  std::map<std::string, std::unique_ptr<panther_utils::MovingAverage<double>>> driver_temp_ma_;
 };
 
 }  // namespace panther_manager
diff --git a/panther_manager/src/safety_manager_node.cpp b/panther_manager/src/safety_manager_node.cpp
index d204690c..6933968d 100644
--- a/panther_manager/src/safety_manager_node.cpp
+++ b/panther_manager/src/safety_manager_node.cpp
@@ -46,15 +46,10 @@ SafetyManagerNode::SafetyManagerNode(
 
   const auto battery_temp_window_len = this->get_parameter("battery.temp.window_len").as_int();
   const auto cpu_temp_window_len = this->get_parameter("cpu.temp.window_len").as_int();
-  const auto driver_temp_window_len = this->get_parameter("driver.temp.window_len").as_int();
 
   battery_temp_ma_ =
     std::make_unique<panther_utils::MovingAverage<double>>(battery_temp_window_len);
   cpu_temp_ma_ = std::make_unique<panther_utils::MovingAverage<double>>(cpu_temp_window_len);
-  front_driver_temp_ma_ =
-    std::make_unique<panther_utils::MovingAverage<double>>(driver_temp_window_len);
-  rear_driver_temp_ma_ =
-    std::make_unique<panther_utils::MovingAverage<double>>(driver_temp_window_len);
 
   const auto safety_initial_blackboard = CreateSafetyInitialBlackboard();
   safety_tree_manager_ = std::make_unique<BehaviorTreeManager>(
@@ -217,13 +212,30 @@ void SafetyManagerNode::BatteryCB(const BatteryStateMsg::SharedPtr battery)
 
 void SafetyManagerNode::DriverStateCB(const DriverStateMsg::SharedPtr driver_state)
 {
-  front_driver_temp_ma_->Roll(driver_state->front.temperature);
-  rear_driver_temp_ma_->Roll(driver_state->rear.temperature);
+  if (driver_state->motor_controllers.empty()) {
+    RCLCPP_WARN(this->get_logger(), "Received empty driver state message.");
+    return;
+  }
+
+  for (auto & driver : driver_state->motor_controllers) {
+    if (driver_temp_ma_.find(driver.name) == driver_temp_ma_.end()) {
+      RCLCPP_DEBUG(
+        this->get_logger(), "Creating moving average for driver '%s'", driver.name.c_str());
+      const auto driver_temp_window_len = this->get_parameter("driver.temp.window_len").as_int();
+      driver_temp_ma_[driver.name] =
+        std::make_unique<panther_utils::MovingAverage<double>>(driver_temp_window_len);
+    }
+
+    driver_temp_ma_[driver.name]->Roll(driver.state.temperature);
+  }
 
   // to simplify conditions pass only higher temp of motor drivers
+  const auto max_element = std::max_element(
+    driver_temp_ma_.begin(), driver_temp_ma_.end(),
+    [](const auto & a, const auto & b) { return a.second->GetAverage() < b.second->GetAverage(); });
+
   safety_tree_manager_->GetBlackboard()->set<double>(
-    "driver_temp",
-    std::max({front_driver_temp_ma_->GetAverage(), rear_driver_temp_ma_->GetAverage()}));
+    "driver_temp", max_element->second->GetAverage());
 }
 
 void SafetyManagerNode::EStopCB(const BoolMsg::SharedPtr e_stop)
diff --git a/panther_manager/test/test_safety_behavior_tree.cpp b/panther_manager/test/test_safety_behavior_tree.cpp
index ab7d978a..50a93343 100644
--- a/panther_manager/test/test_safety_behavior_tree.cpp
+++ b/panther_manager/test/test_safety_behavior_tree.cpp
@@ -30,7 +30,6 @@
 #include "std_srvs/srv/set_bool.hpp"
 #include "std_srvs/srv/trigger.hpp"
 
-#include "panther_msgs/msg/driver_state.hpp"
 #include "panther_msgs/msg/io_state.hpp"
 #include "panther_msgs/msg/system_status.hpp"
 
@@ -40,7 +39,6 @@
 using BoolMsg = std_msgs::msg::Bool;
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
 using IOStateMsg = panther_msgs::msg::IOState;
-using DriverStateMsg = panther_msgs::msg::DriverState;
 using SystemStatusMsg = panther_msgs::msg::SystemStatus;
 using SetBoolSrv = std_srvs::srv::SetBool;
 using TriggerSrv = std_srvs::srv::Trigger;
diff --git a/panther_manager/test/test_safety_manager_node.cpp b/panther_manager/test/test_safety_manager_node.cpp
index f7db1354..ecd497dd 100644
--- a/panther_manager/test/test_safety_manager_node.cpp
+++ b/panther_manager/test/test_safety_manager_node.cpp
@@ -194,9 +194,11 @@ TEST_F(TestSafetyManagerNode, DriverStateCBBlackboardUpdate)
 {
   const float expected_temp = 21.0;
 
+  panther_msgs::msg::MotorController motor_controller;
+  motor_controller.state.temperature = expected_temp;
+
   auto driver_state_msg = panther_msgs::msg::DriverState();
-  driver_state_msg.front.temperature = expected_temp;
-  driver_state_msg.rear.temperature = expected_temp;
+  driver_state_msg.motor_controllers.push_back(motor_controller);
 
   panther_utils::test_utils::PublishAndSpin(
     safety_manager_node_, "hardware/motor_controllers_state", driver_state_msg);

From 6862926f97706745c05ff1dcb616f1e7971764bf Mon Sep 17 00:00:00 2001
From: Dawid Kmak <73443304+KmakD@users.noreply.github.com>
Date: Tue, 20 Aug 2024 08:54:35 +0200
Subject: [PATCH 002/100] Ros2 lynx description (#390)

* Lynx description

* fix dimensions

* fix dimensions

* Update lynx_description/README.md

Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>

* Update lynx_description/config/components.yaml

Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>

* review

---------

Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
---
 lynx_description/CMakeLists.txt               |  11 +
 lynx_description/README.md                    |  12 ++
 lynx_description/config/WH05.yaml             |   9 +
 lynx_description/config/components.yaml       |  25 +++
 lynx_description/hooks/lynx_description.sh.in |   3 +
 lynx_description/launch/load_urdf.launch.py   | 200 ++++++++++++++++++
 lynx_description/package.xml                  |  28 +++
 lynx_description/urdf/body.urdf.xacro         |  64 ++++++
 lynx_description/urdf/gazebo.urdf.xacro       | 136 ++++++++++++
 lynx_description/urdf/lynx.urdf.xacro         |  41 ++++
 lynx_description/urdf/lynx_macro.urdf.xacro   | 190 +++++++++++++++++
 lynx_description/urdf/wheel.urdf.xacro        |  94 ++++++++
 .../config/WH05_controller.yaml               |  84 ++++++++
 13 files changed, 897 insertions(+)
 create mode 100644 lynx_description/CMakeLists.txt
 create mode 100644 lynx_description/README.md
 create mode 100644 lynx_description/config/WH05.yaml
 create mode 100644 lynx_description/config/components.yaml
 create mode 100755 lynx_description/hooks/lynx_description.sh.in
 create mode 100644 lynx_description/launch/load_urdf.launch.py
 create mode 100644 lynx_description/package.xml
 create mode 100644 lynx_description/urdf/body.urdf.xacro
 create mode 100644 lynx_description/urdf/gazebo.urdf.xacro
 create mode 100644 lynx_description/urdf/lynx.urdf.xacro
 create mode 100644 lynx_description/urdf/lynx_macro.urdf.xacro
 create mode 100644 lynx_description/urdf/wheel.urdf.xacro
 create mode 100644 panther_controller/config/WH05_controller.yaml

diff --git a/lynx_description/CMakeLists.txt b/lynx_description/CMakeLists.txt
new file mode 100644
index 00000000..f4e2ded4
--- /dev/null
+++ b/lynx_description/CMakeLists.txt
@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.10.2)
+project(lynx_description)
+
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY config launch urdf DESTINATION share/${PROJECT_NAME})
+
+ament_environment_hooks(
+  "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in")
+
+ament_package()
diff --git a/lynx_description/README.md b/lynx_description/README.md
new file mode 100644
index 00000000..953bab34
--- /dev/null
+++ b/lynx_description/README.md
@@ -0,0 +1,12 @@
+# lynx_description
+
+The package contains URDF files responsible for creating a representation of the robot by specifying the relationships and types of connections (joints) between individual links. It also contains information about the robot's mesh.
+
+## Launch Files
+
+- `load_urdf.launch.py` - loads the robot's URDF and creates simple bindings to display moving joints.
+
+## Configuration Files
+
+- [`components.yaml`](./config/components.yaml): Allows you to quickly add visualization of sensors, TF connections and simulate their behavior in the simulator.
+- [`WH05.yaml`](./config/WH05.yaml): Description of physical and visual parameters for the wheel WH05.
diff --git a/lynx_description/config/WH05.yaml b/lynx_description/config/WH05.yaml
new file mode 100644
index 00000000..01b4967f
--- /dev/null
+++ b/lynx_description/config/WH05.yaml
@@ -0,0 +1,9 @@
+wheel_radius: 0.1445
+wheel_width: 0.09
+wheel_separation: 0.45
+# TODO change inertia and mass when ready
+mass: 2.5
+inertia: { ixx: 0.014738, iyy: 0.0261, izz: 0.014738 }
+inertia_y_offset: 0.0
+mesh_package: lynx_description
+folder_path: meshes/WH05
diff --git a/lynx_description/config/components.yaml b/lynx_description/config/components.yaml
new file mode 100644
index 00000000..71826593
--- /dev/null
+++ b/lynx_description/config/components.yaml
@@ -0,0 +1,25 @@
+# By default lynx is loaded without any components.
+
+components: []
+
+# If you want to add for example LDR01, LDR06 and CAM01 to your Panther look at this example below:
+
+# components:
+#   - type: LDR01
+#     parent_link: cover_link
+#     xyz: 0.0 0.1 0.0
+#     rpy: 0.0 0.0 0.0
+#     device_namespace: main_lidar
+
+#   - type: LDR06
+#     parent_link: cover_link
+#     xyz: 0.0 -0.1 0.0
+#     rpy: 0.0 0.0 0.0
+#     device_namespace: second_lidar
+
+#   - type: CAM01
+#     name: camera
+#     parent_link: cover_link
+#     xyz: 0.1 0.0 0.0
+#     rpy: 0.0 0.0 0.0
+#     device_namespace: front_cam
diff --git a/lynx_description/hooks/lynx_description.sh.in b/lynx_description/hooks/lynx_description.sh.in
new file mode 100755
index 00000000..45d23a05
--- /dev/null
+++ b/lynx_description/hooks/lynx_description.sh.in
@@ -0,0 +1,3 @@
+ament_prepend_unique_value GAZEBO_MODEL_PATH "@CMAKE_INSTALL_PREFIX@/share"
+ament_prepend_unique_value GZ_SIM_RESOURCE_PATH "@CMAKE_INSTALL_PREFIX@/share"
+ament_prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "@CMAKE_INSTALL_PREFIX@/share"
diff --git a/lynx_description/launch/load_urdf.launch.py b/lynx_description/launch/load_urdf.launch.py
new file mode 100644
index 00000000..9823d514
--- /dev/null
+++ b/lynx_description/launch/load_urdf.launch.py
@@ -0,0 +1,200 @@
+#!/usr/bin/env python3
+
+# Copyright 2020 ros2_control Development Team
+# 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.
+
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import (
+    Command,
+    EnvironmentVariable,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression,
+)
+from launch_ros.actions import Node, SetParameter
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    add_wheel_joints = LaunchConfiguration("add_wheel_joints")
+    declared_add_wheel_joints_arg = DeclareLaunchArgument(
+        "add_wheel_joints",
+        default_value="True",
+        description="Flag enabling joint_state_publisher to publish information about the wheel "
+        "position. Should be false when there is a controller that sends this information.",
+        choices=["True", "true", "False", "false"],
+    )
+
+    battery_config_path = LaunchConfiguration("battery_config_path")
+    declare_battery_config_path_arg = DeclareLaunchArgument(
+        "battery_config_path",
+        description=(
+            "Path to the Ignition LinearBatteryPlugin configuration file. "
+            "This configuration is intended for use in simulations only."
+        ),
+        default_value="",
+    )
+
+    components_config_path = LaunchConfiguration("components_config_path")
+    declare_components_config_path_arg = DeclareLaunchArgument(
+        "components_config_path",
+        default_value=PathJoinSubstitution(
+            [FindPackageShare("lynx_description"), "config", "components.yaml"]
+        ),
+        description=(
+            "Additional components configuration file. Components described in this file "
+            "are dynamically included in Lynx's urdf."
+            "Lynx options are described here "
+            "https://husarion.com/manuals/lynx/lynx-options/"
+        ),
+    )
+
+    wheel_type = LaunchConfiguration("wheel_type")  # wheel_type is used by controller_config_path
+    controller_config_path = LaunchConfiguration("controller_config_path")
+    declare_controller_config_path_arg = DeclareLaunchArgument(
+        "controller_config_path",
+        default_value=PathJoinSubstitution(
+            [
+                FindPackageShare("panther_controller"),
+                "config",
+                PythonExpression(["'", wheel_type, "_controller.yaml'"]),
+            ]
+        ),
+        description=(
+            "Path to controller configuration file. By default, it is located in"
+            " 'panther_controller/config/{wheel_type}_controller.yaml'. You can also specify"
+            " the path to your custom controller configuration file here. "
+        ),
+    )
+
+    namespace = LaunchConfiguration("namespace")
+    declare_namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Add namespace to all launched nodes.",
+    )
+
+    use_sim = LaunchConfiguration("use_sim")
+    declare_use_sim_arg = DeclareLaunchArgument(
+        "use_sim",
+        default_value="False",
+        description="Whether simulation is used.",
+        choices=["True", "true", "False", "false"],
+    )
+
+    declare_wheel_config_path_arg = DeclareLaunchArgument(
+        "wheel_config_path",
+        default_value=PathJoinSubstitution(
+            [
+                FindPackageShare("lynx_description"),
+                "config",
+                PythonExpression(["'", wheel_type, ".yaml'"]),
+            ]
+        ),
+        description=(
+            "Path to wheel configuration file. By default, it is located in "
+            "'lynx_description/config/{wheel_type}.yaml'. You can also specify the path "
+            "to your custom wheel configuration file here. "
+        ),
+    )
+
+    wheel_config_path = LaunchConfiguration("wheel_config_path")
+    declare_wheel_type_arg = DeclareLaunchArgument(
+        "wheel_type",
+        default_value="WH05",
+        description=(
+            "Type of wheel. If you choose a value from the preset options ('WH05'), you can "
+            "ignore the 'wheel_config_path' and 'controller_config_path' parameters. "
+            "For custom wheels, please define these parameters to point to files that "
+            "accurately describe the custom wheels."
+        ),
+        choices=["WH05", "custom"],
+    )
+
+    imu_localization_x = os.environ.get("LYNX_IMU_LOCALIZATION_X", "0.168")
+    imu_localization_y = os.environ.get("LYNX_IMU_LOCALIZATION_Y", "0.028")
+    imu_localization_z = os.environ.get("LYNX_IMU_LOCALIZATION_Z", "0.083")
+    imu_orientation_r = os.environ.get("LYNX_IMU_ORIENTATION_R", "3.14")
+    imu_orientation_p = os.environ.get("LYNX_IMU_ORIENTATION_P", "-1.57")
+    imu_orientation_y = os.environ.get("LYNX_IMU_ORIENTATION_Y", "0.0")
+
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("lynx_description"), "urdf", "lynx.urdf.xacro"]
+            ),
+            " use_sim:=",
+            use_sim,
+            " wheel_config_file:=",
+            wheel_config_path,
+            " controller_config_file:=",
+            controller_config_path,
+            " battery_config_file:=",
+            battery_config_path,
+            " imu_xyz:=",
+            f'"{imu_localization_x} {imu_localization_y} {imu_localization_z}"',
+            " imu_rpy:=",
+            f'"{imu_orientation_r} {imu_orientation_p} {imu_orientation_y}"',
+            " namespace:=",
+            namespace,
+            " components_config_path:=",
+            components_config_path,
+        ]
+    )
+
+    namespace_ext = PythonExpression(["'", namespace, "' + '/' if '", namespace, "' else ''"])
+
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        parameters=[
+            {"robot_description": robot_description_content},
+            {"frame_prefix": namespace_ext},
+        ],
+        namespace=namespace,
+        emulate_tty=True,
+    )
+
+    joint_state_publisher_node = Node(
+        package="joint_state_publisher",
+        executable="joint_state_publisher",
+        namespace=namespace,
+        emulate_tty=True,
+        condition=IfCondition(add_wheel_joints),
+    )
+
+    actions = [
+        declared_add_wheel_joints_arg,
+        declare_battery_config_path_arg,
+        declare_components_config_path_arg,
+        declare_wheel_type_arg,  # wheel_type is used by controller_config_path
+        declare_controller_config_path_arg,
+        declare_namespace_arg,
+        declare_use_sim_arg,
+        declare_wheel_config_path_arg,
+        SetParameter(name="use_sim_time", value=use_sim),
+        robot_state_pub_node,
+        joint_state_publisher_node,  # do not publish, when use_sim is true
+    ]
+
+    return LaunchDescription(actions)
diff --git a/lynx_description/package.xml b/lynx_description/package.xml
new file mode 100644
index 00000000..77584066
--- /dev/null
+++ b/lynx_description/package.xml
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lynx_description</name>
+  <version>2.1.0</version>
+  <description>The lynx_description package</description>
+  <maintainer email="support@husarion.com">Husarion</maintainer>
+  <license>Apache License 2.0</license>
+
+  <url type="website">https://husarion.com/</url>
+  <url type="repository">https://github.com/husarion/panther_ros</url>
+  <url type="bugtracker">https://github.com/husarion/panther_ros/issues</url>
+
+  <author email="dawid.kmak@husarion.com">Dawid Kmak</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>ros_components_description</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/lynx_description/urdf/body.urdf.xacro b/lynx_description/urdf/body.urdf.xacro
new file mode 100644
index 00000000..c424ba01
--- /dev/null
+++ b/lynx_description/urdf/body.urdf.xacro
@@ -0,0 +1,64 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+
+  <!-- body defining macro -->
+  <xacro:macro
+    name="body"
+    params="wheel_radius imu_xyz imu_rpy">
+
+    <link name="base_link" />
+
+    <joint name="base_to_body_joint" type="fixed">
+      <origin xyz="0.0 0.0 ${wheel_radius}" rpy="0.0 0.0 0.0" />
+      <parent link="base_link" />
+      <child link="body_link" />
+    </joint>
+
+    <link name="body_link">
+      <visual>
+        <origin xyz="0.0 0.0 0.0745" rpy="0.0 0.0 0.0" />
+        <geometry>
+          <!-- TODO cheange when models ready -->
+          <!-- <mesh filename="package://lynx_description/meshes/body.dae" /> -->
+          <box size="0.646 0.34 0.268" />
+        </geometry>
+      </visual>
+
+      <collision>
+        <origin xyz="0.0 0.0 0.0745" rpy="0.0 0.0 0.0" />
+        <geometry>
+          <!-- TODO cheange when models ready -->
+          <!-- <mesh filename="package://lynx_description/meshes/body_collision.stl" /> -->
+          <box size="0.646 0.34 0.268" />
+        </geometry>
+      </collision>
+
+      <inertial>
+        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+        <mass value="30.0" />
+        <!-- TODO calculate real interia -->
+        <inertia ixx="0.46856"  ixy="0.0"     ixz="0.0"
+                                iyy="1.22285" iyz="0.0"
+                                              izz="1.51185" />
+      </inertial>
+    </link>
+
+    <joint name="body_to_imu_joint" type="fixed">
+      <origin xyz="${imu_xyz}" rpy="${imu_rpy}" />
+      <parent link="body_link" />
+      <child link="imu_link" />
+    </joint>
+
+    <link name="imu_link" />
+
+    <joint name="body_to_cover_joint" type="fixed">
+      <!-- TODO cheange to real value -->
+      <origin xyz="0.0 0.0 0.2085" rpy="0.0 0.0 0.0" />
+      <parent link="body_link" />
+      <child link="cover_link" />
+    </joint>
+
+    <link name="cover_link" />
+
+  </xacro:macro>
+</robot>
diff --git a/lynx_description/urdf/gazebo.urdf.xacro b/lynx_description/urdf/gazebo.urdf.xacro
new file mode 100644
index 00000000..b8b2afdb
--- /dev/null
+++ b/lynx_description/urdf/gazebo.urdf.xacro
@@ -0,0 +1,136 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+
+  <!-- Battery -->
+  <xacro:macro name="battery" params="config_file namespace:=''">
+
+    <!-- Read property -->
+    <xacro:property name="battery_config" value="${xacro.load_yaml(config_file)}" />
+    <xacro:if value="${namespace == ''}">
+      <xacro:property name="ns" value="" />
+    </xacro:if>
+    <xacro:unless value="${namespace == ''}">
+        <xacro:property name="ns" value="${namespace}/" />
+    </xacro:unless>
+
+    <!-- Sim Battery -->
+    <gazebo>
+      <plugin filename="ignition-gazebo-linearbatteryplugin-system"
+        name="gz::sim::systems::LinearBatteryPlugin">
+        <battery_name>${ns}battery</battery_name>
+        <voltage>41.4</voltage>
+        <open_circuit_voltage_constant_coef>42.0</open_circuit_voltage_constant_coef>
+        <open_circuit_voltage_linear_coef>-10.0</open_circuit_voltage_linear_coef>
+        <initial_charge>${battery_config['initial_charge_percentage']*battery_config['capacity']}</initial_charge>
+        <capacity>${battery_config['capacity']}</capacity>
+        <resistance>0.15</resistance>
+        <smooth_current_tau>2.0</smooth_current_tau>
+        <enable_recharge>true</enable_recharge>
+        <charging_time>${battery_config['charging_time']}</charging_time>
+        <soc_threshold>2.0</soc_threshold>
+        <!-- TODO Because of https://github.com/gazebosim/gz-sim/issues/225 division by 100 -->
+        <power_load>${battery_config['power_load']/100.0}</power_load>
+        <start_draining>${battery_config['simulate_discharging']}</start_draining>
+      </plugin>
+    </gazebo>
+  </xacro:macro>
+
+  <!-- Controller -->
+  <xacro:macro name="controller" params="config_file namespace:=''">
+    <gazebo>
+      <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+        <parameters>${config_file}</parameters>
+        <ros>
+          <namespace>${namespace}</namespace>
+          <remapping>imu_broadcaster/imu:=imu/data</remapping>
+          <remapping>drive_controller/cmd_vel_unstamped:=cmd_vel</remapping>
+          <remapping>drive_controller/odom:=odometry/wheels</remapping>
+        </ros>
+      </plugin>
+    </gazebo>
+  </xacro:macro>
+
+  <!-- Gauss noise macro -->
+  <xacro:macro name="gauss_noise"
+    params="mean:=0.0 stddev:=0.0 bias_mean:=0.0 bias_stddev:=0.0 precision:=0.0">
+    <noise type="gaussian">
+      <mean>${mean}</mean>
+      <stddev>${stddev}</stddev>
+      <bias_mean>${bias_mean}</bias_mean>
+      <bias_stddev>${bias_stddev}</bias_stddev>
+      <precision>${precision}</precision>
+    </noise>
+  </xacro:macro>
+
+  <!-- IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications -->
+  <xacro:macro name="imu" params="reference_frame namespace:=''">
+
+    <xacro:if value="${namespace == ''}">
+      <xacro:property name="ns" value="" />
+    </xacro:if>
+    <xacro:unless value="${namespace == ''}">
+        <xacro:property name="ns" value="${namespace}/" />
+    </xacro:unless>
+
+    <gazebo reference="${reference_frame}">
+      <sensor name="${ns}imu" type="imu">
+        <always_on>true</always_on>
+        <update_rate>50</update_rate>
+        <topic>${ns}imu/data_raw</topic>
+        <visualize>false</visualize>
+        <enable_metrics>false</enable_metrics>
+        <ignition_frame_id>imu_link</ignition_frame_id>
+        <imu>
+          <orientation_reference_frame>
+            <localization>ENU</localization>
+          </orientation_reference_frame>
+          <angular_velocity>
+            <!-- rad/s -->
+            <x>
+              <xacro:gauss_noise stddev="0.01" bias_mean="3.3e-5" precision="1.2e-3" />
+            </x>
+            <y>
+              <xacro:gauss_noise stddev="0.01" bias_mean="3.3e-5" precision="1.2e-3" />
+            </y>
+            <z>
+              <xacro:gauss_noise stddev="0.01" bias_mean="3.3e-5" precision="1.2e-3" />
+            </z>
+          </angular_velocity>
+          <linear_acceleration>
+            <!-- m/s^2 -->
+            <x>
+              <xacro:gauss_noise stddev="27.5e-3" bias_mean="18.6e-3" precision="9.6e-3" />
+            </x>
+            <y>
+              <xacro:gauss_noise stddev="27.5e-3" bias_mean="18.6e-3" precision="9.6e-3" />
+            </y>
+            <z>
+              <xacro:gauss_noise stddev="27.5e-3" bias_mean="18.6e-3" precision="9.6e-3" />
+            </z>
+          </linear_acceleration>
+        </imu>
+      </sensor>
+    </gazebo>
+  </xacro:macro>
+
+
+  <!-- Lights -->
+  <xacro:macro name="light" params="reference_frame name namespace:=''">
+    <gazebo reference="${reference_frame}">
+      <light name="${name}" type="spot">
+        <cast_shadows>true</cast_shadows>
+        <visualize>0</visualize>
+        <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>
+        <spot>
+          <inner_angle>1.0</inner_angle>
+          <outer_angle>2.0</outer_angle>
+          <falloff>0.4</falloff>
+        </spot>
+      </light>
+    </gazebo>
+  </xacro:macro>
+
+</robot>
diff --git a/lynx_description/urdf/lynx.urdf.xacro b/lynx_description/urdf/lynx.urdf.xacro
new file mode 100644
index 00000000..557b8eae
--- /dev/null
+++ b/lynx_description/urdf/lynx.urdf.xacro
@@ -0,0 +1,41 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="lynx" xmlns:xacro="http://wiki.ros.org/xacro">
+
+  <xacro:arg name="use_sim" default="false" />
+  <!-- if lidars have to work correctly, use_gpu has to be true. -->
+  <xacro:arg name="use_gpu" default="true" />
+  <!-- TODO find IMU position -->
+  <xacro:arg name="imu_xyz" default="0.169 0.025 0.092" />
+  <xacro:arg name="imu_rpy" default="0.0 0.0 -1.57" />
+  <xacro:arg name="wheel_config_file"
+    default="$(find lynx_description)/config/WH05.yaml" />
+  <xacro:arg name="controller_config_file"
+    default="$(find panther_controller)/config/WH05_controller.yaml" />
+  <xacro:arg name="battery_config_file"
+    default="" />
+  <xacro:arg name="namespace" default="" />
+
+
+  <xacro:include filename="$(find lynx_description)/urdf/lynx_macro.urdf.xacro" ns="husarion" />
+  <xacro:husarion.lynx_robot
+    use_sim="$(arg use_sim)"
+    imu_xyz="$(arg imu_xyz)"
+    imu_rpy="$(arg imu_rpy)"
+    wheel_config_file="$(arg wheel_config_file)"
+    controller_config_file="$(arg controller_config_file)"
+    battery_config_file="$(arg battery_config_file)"
+    namespace="$(arg namespace)" />
+
+  <xacro:arg name="components_config_path" default="$(find lynx_description)/config/components.yaml" />
+  <xacro:property name="components_config_path_property" value="$(arg components_config_path)" />
+
+  <xacro:unless value="${components_config_path_property == 'None'}">
+    <xacro:include filename="$(find ros_components_description)/urdf/components.urdf.xacro"
+      ns="husarion_components" />
+
+    <xacro:husarion_components.create_components
+      components_config_path="${components_config_path_property}"
+      namespace="$(arg namespace)"
+    />
+  </xacro:unless>
+</robot>
diff --git a/lynx_description/urdf/lynx_macro.urdf.xacro b/lynx_description/urdf/lynx_macro.urdf.xacro
new file mode 100644
index 00000000..94b691ae
--- /dev/null
+++ b/lynx_description/urdf/lynx_macro.urdf.xacro
@@ -0,0 +1,190 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+
+  <xacro:macro
+    name="lynx_robot"
+    params="wheel_config_file
+            controller_config_file
+            battery_config_file
+            use_sim:=false
+            imu_xyz:='0.169 0.025 0.092'
+            imu_rpy:='0.0 0.0 -1.57'
+            namespace:=''">
+
+    <xacro:if value="${namespace == ''}">
+      <xacro:property name="ns" value="" />
+    </xacro:if>
+    <xacro:unless value="${namespace == ''}">
+        <xacro:property name="ns" value="${namespace}/" />
+    </xacro:unless>
+
+    <xacro:property name="wheel_config" value="${xacro.load_yaml(wheel_config_file)}" />
+    <!-- TODO find real value -->
+    <xacro:property name="wheel_separation_x" value="0.35" />
+
+    <!-- INCLUDE ROBOT PARTS DEFINITIONS -->
+    <xacro:include filename="$(find lynx_description)/urdf/body.urdf.xacro" ns="body" />
+    <xacro:include filename="$(find lynx_description)/urdf/gazebo.urdf.xacro" ns="gazebo" />
+    <xacro:include filename="$(find lynx_description)/urdf/wheel.urdf.xacro" ns="wheel" />
+
+    <!-- BODY DECLARATION -->
+    <xacro:body.body
+      wheel_radius="${wheel_config['wheel_radius']}"
+      imu_xyz="${imu_xyz}"
+      imu_rpy="${imu_rpy}" />
+
+    <!-- WHEEL DECLARATION -->
+    <xacro:wheel.wheel
+      wheel_config="${wheel_config}"
+      wheel_separation_x="${wheel_separation_x}"
+      prefix="fl" />
+
+    <xacro:wheel.wheel
+      wheel_config="${wheel_config}"
+      wheel_separation_x="${wheel_separation_x}"
+      prefix="fr" />
+
+    <xacro:wheel.wheel
+      wheel_config="${wheel_config}"
+      wheel_separation_x="${wheel_separation_x}"
+      prefix="rl" />
+
+    <xacro:wheel.wheel
+      wheel_config="${wheel_config}"
+      wheel_separation_x="${wheel_separation_x}"
+      prefix="rr" />
+
+    <ros2_control name="${ns}lynx_system" type="system">
+      <hardware>
+        <xacro:if value="${use_sim}">
+          <plugin>ign_ros2_control/IgnitionSystem</plugin>
+        </xacro:if>
+
+        <xacro:unless value="$(arg use_sim)">
+          <plugin>panther_hardware_interfaces/PantherSystem</plugin>
+
+          <param name="encoder_resolution">1600</param>
+
+          <param name="gear_ratio">30.08</param>
+          <param name="gearbox_efficiency">0.75</param>
+
+          <!-- Same as set in the Roboteq driver (TNM parameter) -->
+          <param name="motor_torque_constant">0.11</param>
+
+          <!-- Max RPM speed set in the Roboteq driver (MXRPM parameter) -->
+          <param name="max_rpm_motor_speed">3600.0</param>
+
+          <param name="can_interface_name">panther_can</param>
+          <param name="master_can_id">3</param>
+          <param name="front_driver_can_id">1</param>
+          <param name="rear_driver_can_id">2</param>
+          <param name="sdo_operation_timeout_ms">100</param>
+
+          <!-- Depends on frequency of the controller, more critical motors state are sent with
+            higher frequency, other parameters are sent with lower, here allowed time is set
+            to be expected period +50% margin -->
+          <param name="pdo_motor_states_timeout_ms">15</param>
+          <param name="pdo_driver_state_timeout_ms">75</param>
+
+          <!-- It will be rounded to the closest value taking into account current controller frequency -->
+          <param name="driver_states_update_frequency">20.0</param>
+
+          <param name="max_roboteq_initialization_attempts">5</param>
+          <param name="max_roboteq_activation_attempts">5</param>
+
+          <!-- TODO after all tests update parameters, these ones are quite high for the worst case scenario -->
+          <param name="max_write_pdo_cmds_errors_count">2</param>
+          <param name="max_read_pdo_motor_states_errors_count">2</param>
+          <param name="max_read_pdo_driver_state_errors_count">2</param>
+        </xacro:unless>
+      </hardware>
+
+      <joint name="fl_wheel_joint">
+        <command_interface name="velocity" />
+        <state_interface name="position" />
+        <state_interface name="velocity" />
+        <state_interface name="effort" />
+      </joint>
+      <joint name="fr_wheel_joint">
+        <command_interface name="velocity" />
+        <state_interface name="position" />
+        <state_interface name="velocity" />
+        <state_interface name="effort" />
+      </joint>
+      <joint name="rl_wheel_joint">
+        <command_interface name="velocity" />
+        <state_interface name="position" />
+        <state_interface name="velocity" />
+        <state_interface name="effort" />
+      </joint>
+      <joint name="rr_wheel_joint">
+        <command_interface name="velocity" />
+        <state_interface name="position" />
+        <state_interface name="velocity" />
+        <state_interface name="effort" />
+      </joint>
+
+      <xacro:if value="${use_sim}">
+        <sensor name="${ns}imu">
+          <state_interface name="orientation.x" />
+          <state_interface name="orientation.y" />
+          <state_interface name="orientation.z" />
+          <state_interface name="orientation.w" />
+          <state_interface name="angular_velocity.x" />
+          <state_interface name="angular_velocity.y" />
+          <state_interface name="angular_velocity.z" />
+          <state_interface name="linear_acceleration.x" />
+          <state_interface name="linear_acceleration.y" />
+          <state_interface name="linear_acceleration.z" />
+        </sensor>
+      </xacro:if>
+    </ros2_control>
+
+    <xacro:unless value="$(arg use_sim)">
+      <ros2_control name="${ns}imu" type="sensor">
+        <hardware>
+          <plugin>panther_hardware_interfaces/PantherImuSensor</plugin>
+          <param name="serial">-1</param>
+          <param name="hub_port">0</param>
+          <param name="data_interval_ms">8</param>
+          <param name="callback_delta_epsilon_ms">1</param>
+
+          <!-- Madgwick Filter Params -->
+          <param name="gain">0.00304</param>
+          <param name="zeta">0.00151</param>
+          <param name="mag_bias_x">0.0</param>
+          <param name="mag_bias_y">0.0</param>
+          <param name="mag_bias_z">0.0</param>
+          <param name="use_mag">true</param>
+          <param name="stateless">false</param>
+          <param name="remove_gravity_vector">false</param>
+          <param name="world_frame">enu</param>
+        </hardware>
+
+        <sensor name="${ns}imu">
+          <state_interface name="orientation.x" />
+          <state_interface name="orientation.y" />
+          <state_interface name="orientation.z" />
+          <state_interface name="orientation.w" />
+          <state_interface name="angular_velocity.x" />
+          <state_interface name="angular_velocity.y" />
+          <state_interface name="angular_velocity.z" />
+          <state_interface name="linear_acceleration.x" />
+          <state_interface name="linear_acceleration.y" />
+          <state_interface name="linear_acceleration.z" />
+        </sensor>
+      </ros2_control>
+    </xacro:unless>
+
+    <xacro:if value="${use_sim}">
+      <xacro:if value="${battery_config_file != ''}">
+        <xacro:gazebo.battery config_file="${battery_config_file}" namespace="${namespace}" />
+      </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:if>
+  </xacro:macro>
+
+</robot>
diff --git a/lynx_description/urdf/wheel.urdf.xacro b/lynx_description/urdf/wheel.urdf.xacro
new file mode 100644
index 00000000..40dfbe54
--- /dev/null
+++ b/lynx_description/urdf/wheel.urdf.xacro
@@ -0,0 +1,94 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+
+  <!-- wheel defining macro -->
+  <xacro:macro name="wheel" params="wheel_config wheel_separation_x prefix">
+
+    <xacro:if value="${prefix == 'fl'}">
+      <xacro:property name="x" value="${wheel_separation_x/2.0}" />
+      <xacro:property name="y" value="${wheel_config['wheel_separation']/2.0}" />
+      <xacro:property name="inertia_y_offset" value="${wheel_config['inertia_y_offset']}" />
+      <xacro:property name="fdir" value="1 -1 0" />
+    </xacro:if>
+    <xacro:if value="${prefix == 'fr'}">
+      <xacro:property name="x" value="${wheel_separation_x/2.0}" />
+      <xacro:property name="y" value="-${wheel_config['wheel_separation']/2.0}" />
+      <xacro:property name="inertia_y_offset" value="${-wheel_config['inertia_y_offset']}" />
+      <xacro:property name="fdir" value="1 1 0" />
+    </xacro:if>
+    <xacro:if value="${prefix == 'rl'}">
+      <xacro:property name="x" value="-${wheel_separation_x/2.0}" />
+      <xacro:property name="y" value="${wheel_config['wheel_separation']/2.0}" />
+      <xacro:property name="inertia_y_offset" value="${wheel_config['inertia_y_offset']}" />
+      <xacro:property name="fdir" value="1 1 0" />
+    </xacro:if>
+    <xacro:if value="${prefix == 'rr'}">
+      <xacro:property name="x" value="-${wheel_separation_x/2.0}" />
+      <xacro:property name="y" value="-${wheel_config['wheel_separation']/2.0}" />
+      <xacro:property name="inertia_y_offset" value="${-wheel_config['inertia_y_offset']}" />
+      <xacro:property name="fdir" value="1 -1 0" />
+    </xacro:if>
+
+    <joint name="${prefix}_wheel_joint" type="continuous">
+      <origin xyz="${x} ${y} 0.0" rpy="0.0 0.0 0.0" />
+      <parent link="body_link" />
+      <child link="${prefix}_wheel_link" />
+      <axis xyz="0.0 1.0 0.0" />
+
+      <limit effort="64.5" velocity="10.958" />
+      <dynamics damping="0.01" friction="0.1" />
+    </joint>
+
+    <link name="${prefix}_wheel_link">
+      <visual>
+        <origin xyz="0.0 0.0 0.0" rpy="-${pi/2} 0.0 0.0" />
+        <geometry>
+          <!-- TODO cheange when models ready -->
+          <!-- <mesh
+          filename="package://${wheel_config['mesh_package']}/${wheel_config['folder_path']}/${prefix}_wheel.dae"
+          /> -->
+          <cylinder radius="${wheel_config['wheel_radius']}" length="${wheel_config['wheel_width']}" />
+        </geometry>
+      </visual>
+
+      <collision>
+        <origin xyz="0.0 0.0 0.0" rpy="-${pi/2} 0.0 0.0" />
+        <geometry>
+          <cylinder radius="${wheel_config['wheel_radius']}" length="${wheel_config['wheel_width']}" />
+        </geometry>
+      </collision>
+
+      <inertial>
+        <origin xyz="0.0 ${inertia_y_offset} 0.0" rpy="0.0 0.0 0.0" />
+        <mass value="${wheel_config['mass']}" />
+        <inertia
+          ixx="${wheel_config['inertia']['ixx']}"
+          ixy="0.0"
+          ixz="0.0"
+          iyy="${wheel_config['inertia']['iyy']}"
+          iyz="0.0"
+          izz="${wheel_config['inertia']['izz']}" />
+      </inertial>
+    </link>
+
+    <xacro:if value="${use_sim}">
+      <xacro:if value="${wheel_config['mecanum']}">
+        <gazebo reference="${prefix}_wheel_link" xmlns:ignition="http://ignitionrobotics.org/schema">
+          <collision>
+            <surface>
+              <friction>
+                <ode>
+                  <mu>1.0</mu>
+                  <mu2>0.0</mu2>
+                  <fdir1 ignition:expressed_in="base_link">${fdir}</fdir1>
+                </ode>
+              </friction>
+            </surface>
+          </collision>
+        </gazebo>
+      </xacro:if>
+    </xacro:if>
+
+  </xacro:macro>
+
+</robot>
\ No newline at end of file
diff --git a/panther_controller/config/WH05_controller.yaml b/panther_controller/config/WH05_controller.yaml
new file mode 100644
index 00000000..f243bdd0
--- /dev/null
+++ b/panther_controller/config/WH05_controller.yaml
@@ -0,0 +1,84 @@
+/**:
+  controller_manager:
+    ros__parameters:
+      update_rate: 100
+
+      joint_state_broadcaster:
+        type: joint_state_broadcaster/JointStateBroadcaster
+      drive_controller:
+        type: diff_drive_controller/DiffDriveController
+      imu_broadcaster:
+        type: imu_sensor_broadcaster/IMUSensorBroadcaster
+
+  # IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications
+  imu_broadcaster:
+    ros__parameters:
+      use_namespace_as_sensor_name_prefix: true
+
+      sensor_name: imu
+      frame_id: imu_link
+      # orientation_stddev: 4.3e-2 rad determined experimentally
+      static_covariance_orientation: [1.8e-3, 0.0, 0.0, 0.0, 1.8e-3, 0.0, 0.0, 0.0, 1.8e-3]
+      # angular_velocity_stdev: 0.59 deg/s (0.01 rad/s) gyroscope white noise sigma, according to the manual
+      static_covariance_angular_velocity: [1.0e-4, 0.0, 0.0, 0.0, 1.0e-4, 0.0, 0.0, 0.0, 1.0e-4]
+      # linear_acceleration_stdev: 2.8 mg (0.0275 m/s^2) accelerometer white noise sigma, according to the manual
+      static_covariance_linear_acceleration: [7.6e-4, 0.0, 0.0, 0.0, 7.6e-4, 0.0, 0.0, 0.0, 7.6e-4]
+
+  drive_controller:
+    ros__parameters:
+      left_wheel_names: [fl_wheel_joint, rl_wheel_joint]
+      right_wheel_names: [fr_wheel_joint, rr_wheel_joint]
+
+      wheel_separation: 0.45
+      wheel_radius: 0.1445
+
+      # For skid drive kinematics it will act as ICR coefficient, kinematic model with ICR
+      # coefficient isn't totally accurate and this coefficient can differ for various ground types
+      wheel_separation_multiplier: 1.5
+
+      left_wheel_radius_multiplier: 1.0
+      right_wheel_radius_multiplier: 1.0
+
+      publish_rate: 100.0
+      odom_frame_id: odom
+      base_frame_id: base_link
+      twist_covariance_diagonal: [5.4e-5, 5.4e-5, 0.0, 0.0, 0.0, 1.9e-4] # Values measured experimentally
+
+      # Whether to use feedback or commands for odometry calculations
+      open_loop: false
+
+      # Update odometry from velocity
+      # in sensor fusion only velocity is used and with this setting it is more accurate
+      position_feedback: false
+      # velocity computation filtering
+      velocity_rolling_window_size: 1
+
+      enable_odom_tf: false
+
+      cmd_vel_timeout: 0.5
+      #publish_limited_velocity: true
+      use_stamped_vel: false
+
+      # Velocity and acceleration limits
+      # Whenever a min_* is unspecified, default to -max_*
+      linear:
+        x:
+          has_velocity_limits: true
+          has_acceleration_limits: true
+          has_jerk_limits: false
+          max_velocity: 1.5 # m/s
+          # min_velocity -  When unspecified `min_velocity=-max_velocity`
+          max_acceleration: 2.1 # m/s^2
+          # min_acceleration - When unspecified, `min_acceleration=-max_acceleration`
+          max_jerk: 0.0 # m/s^3
+
+      angular:
+        z:
+          has_velocity_limits: true
+          has_acceleration_limits: true
+          has_jerk_limits: false
+          max_velocity: 4.0 # rad/s
+          # min_velocity -  When unspecified `min_velocity=-max_velocity`
+          max_acceleration: 5.74 # rad/s^2
+          # min_acceleration - When unspecified, `min_acceleration=-max_acceleration`
+          max_jerk: 0.0 # rad/s^3

From 2b250a90777d32828eaa44bbbd894543b7d34272 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Tue, 20 Aug 2024 08:16:32 +0000
Subject: [PATCH 003/100] roboteq driver abstract class

---
 .../motors_controller/roboteq_driver.hpp      | 103 +++++++++++++++---
 1 file changed, 90 insertions(+), 13 deletions(-)

diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
index 498231c2..081201e1 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
@@ -64,9 +64,86 @@ struct RoboteqDriverState
 };
 
 /**
- * @brief Implementation of LoopDriver for Roboteq drivers
+ * @brief Abstract class for Roboteq driver
  */
-class RoboteqDriver : public lely::canopen::LoopDriver
+class RoboteqDriverInterface
+{
+public:
+  /**
+   * @brief Triggers boot operations
+   *
+   * @exception std::runtime_error if triggering boot fails
+   */
+  virtual std::future<void> Boot() = 0;
+
+  /**
+   * @brief Returns true if CAN error was detected.
+   */
+  virtual bool IsCANError() const = 0;
+
+  /**
+   * @brief Returns true if heartbeat timeout encountered.
+   */
+  virtual bool IsHeartbeatTimeout() const = 0;
+
+  /**
+   * @brief Reads motors' state data returned from Roboteq (PDO 1 and 2) and saves
+   * last timestamps
+   */
+  virtual RoboteqMotorsStates ReadRoboteqMotorsStates() = 0;
+
+  /**
+   * @brief Reads driver state data returned from Roboteq (PDO 3 and 4): error flags, battery
+   * voltage, battery currents (for channel 1 and 2, they are not the same as motor currents),
+   * temperatures. Also saves the last timestamps
+   */
+  virtual RoboteqDriverState ReadRoboteqDriverState() = 0;
+
+  /**
+   * @brief Sends commands to the motors
+   *
+   * @param cmd command value in the range [-1000, 1000]
+   *
+   * @exception std::runtime_error if operation fails
+   */
+  virtual void SendRoboteqCmd(
+    const std::int32_t cmd_channel_1, const std::int32_t cmd_channel_2) = 0;
+
+  /**
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void ResetRoboteqScript() = 0;
+
+  /**
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void TurnOnEStop() = 0;
+
+  /**
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void TurnOffEStop() = 0;
+
+  /**
+   * @brief Sends a safety stop command to the motor connected to channel 1
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void TurnOnSafetyStopChannel1() = 0;
+
+  /**
+   * @brief Sends a safety stop command to the motor connected to channel 2
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void TurnOnSafetyStopChannel2() = 0;
+};
+
+/**
+ * @brief Hardware implementation of RoboteqDriverInterface with lely LoopDriver for Roboteq drivers
+ * control
+ */
+class RoboteqDriver : public RoboteqDriverInterface, public lely::canopen::LoopDriver
 {
 public:
   RoboteqDriver(
@@ -78,30 +155,30 @@ class RoboteqDriver : public lely::canopen::LoopDriver
    *
    * @exception std::runtime_error if triggering boot fails
    */
-  std::future<void> Boot();
+  std::future<void> Boot() override;
 
   /**
    * @brief Returns true if CAN error was detected.
    */
-  bool IsCANError() const { return can_error_.load(); }
+  bool IsCANError() const override { return can_error_.load(); };
 
   /**
    * @brief Returns true if heartbeat timeout encountered.
    */
-  bool IsHeartbeatTimeout() const { return heartbeat_timeout_.load(); }
+  bool IsHeartbeatTimeout() const override { return heartbeat_timeout_.load(); };
 
   /**
    * @brief Reads motors' state data returned from Roboteq (PDO 1 and 2) and saves
    * last timestamps
    */
-  RoboteqMotorsStates ReadRoboteqMotorsStates();
+  RoboteqMotorsStates ReadRoboteqMotorsStates() override;
 
   /**
    * @brief Reads driver state data returned from Roboteq (PDO 3 and 4): error flags, battery
    * voltage, battery currents (for channel 1 and 2, they are not the same as motor currents),
    * temperatures. Also saves the last timestamps
    */
-  RoboteqDriverState ReadRoboteqDriverState();
+  RoboteqDriverState ReadRoboteqDriverState() override;
 
   /**
    * @brief Sends commands to the motors
@@ -110,36 +187,36 @@ class RoboteqDriver : public lely::canopen::LoopDriver
    *
    * @exception std::runtime_error if operation fails
    */
-  void SendRoboteqCmd(const std::int32_t cmd_channel_1, const std::int32_t cmd_channel_2);
+  void SendRoboteqCmd(const std::int32_t cmd_channel_1, const std::int32_t cmd_channel_2) override;
 
   /**
    * @exception std::runtime_error if any operation returns error
    */
-  void ResetRoboteqScript();
+  void ResetRoboteqScript() override;
 
   /**
    * @exception std::runtime_error if any operation returns error
    */
-  void TurnOnEStop();
+  void TurnOnEStop() override;
 
   /**
    * @exception std::runtime_error if any operation returns error
    */
-  void TurnOffEStop();
+  void TurnOffEStop() override;
 
   /**
    * @brief Sends a safety stop command to the motor connected to channel 1
    *
    * @exception std::runtime_error if any operation returns error
    */
-  void TurnOnSafetyStopChannel1();
+  void TurnOnSafetyStopChannel1() override;
 
   /**
    * @brief Sends a safety stop command to the motor connected to channel 2
    *
    * @exception std::runtime_error if any operation returns error
    */
-  void TurnOnSafetyStopChannel2();
+  void TurnOnSafetyStopChannel2() override;
 
 private:
   /**

From c00c9a400a793f4845d3c7ba0f0189ef805e83df Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Tue, 20 Aug 2024 12:51:54 +0000
Subject: [PATCH 004/100] canopen controller abstraction

---
 panther_hardware_interfaces/CMakeLists.txt    |   3 +-
 .../motors_controller/canopen_controller.hpp  |  32 +++-
 .../motors_controller/canopen_controller.cpp  |  61 ++++----
 .../test_canopen_controller.cpp               | 141 +++++++++++++-----
 4 files changed, 161 insertions(+), 76 deletions(-)

diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt
index 8713f699..17362241 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/panther_hardware_interfaces/CMakeLists.txt
@@ -83,6 +83,7 @@ install(
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(panther_utils REQUIRED)
 
@@ -117,7 +118,7 @@ if(BUILD_TESTING)
   target_link_libraries(${PROJECT_NAME}_test_roboteq_data_converters
                         PkgConfig::LIBLELY_COAPP)
 
-  ament_add_gtest(
+  ament_add_gmock(
     ${PROJECT_NAME}_test_canopen_controller
     test/panther_system/motors_controller/test_canopen_controller.cpp
     src/panther_system/motors_controller/canopen_controller.cpp
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp
index 883d7363..f48f7f26 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp
@@ -74,8 +74,7 @@ class CANopenController
    */
   void Deinitialize();
 
-  std::shared_ptr<RoboteqDriver> GetFrontDriver() { return front_driver_; }
-  std::shared_ptr<RoboteqDriver> GetRearDriver() { return rear_driver_; }
+  virtual std::shared_ptr<RoboteqDriverInterface> GetDriver(const std::string & name) = 0;
 
 private:
   void InitializeCANCommunication();
@@ -89,11 +88,16 @@ class CANopenController
   void NotifyCANCommunicationStarted(const bool result);
 
   /**
-   * @brief Triggers boot on front and rear Roboteq drivers and waits for finish
+   * @brief Triggers boot on all defined Roboteq drivers and waits for finish
    *
    * @exception std::runtime_error if boot fails
    */
-  void BootDrivers();
+  virtual void BootDrivers() = 0;
+
+  /**
+   * @brief Initializes all defined Roboteq drivers
+   */
+  virtual void InitializeDrivers() = 0;
 
   // Priority set to be higher than the priority of the main ros2 control node (50)
   static constexpr unsigned kCANopenThreadSchedPriority = 60;
@@ -115,12 +119,26 @@ class CANopenController
   std::shared_ptr<lely::io::CanChannel> chan_;
   std::shared_ptr<lely::canopen::AsyncMaster> master_;
 
-  std::shared_ptr<RoboteqDriver> front_driver_;
-  std::shared_ptr<RoboteqDriver> rear_driver_;
-
   const CANopenSettings canopen_settings_;
 };
 
+// class PantherCANopenController : public CANopenController
+// {
+// public:
+//   PantherCANopenController(const CANopenSettings & canopen_settings);
+
+//   std::shared_ptr<RoboteqDriverInterface> GetDriver(const std::string & name) override;
+
+// private:
+//   void BootDrivers() override;
+//   void InitializeDrivers() override;
+
+//   std::shared_ptr<RoboteqDriver> front_driver_;
+//   std::shared_ptr<RoboteqDriver> rear_driver_;
+
+//   void ResetDrivers();
+// };
+
 }  // namespace panther_hardware_interfaces
 
 #endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_CONTROLLER_HPP_
diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp b/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp
index 18f4117c..eb594d8d 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp
+++ b/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp
@@ -101,8 +101,6 @@ void CANopenController::Deinitialize()
 
   canopen_communication_started_.store(false);
 
-  rear_driver_.reset();
-  front_driver_.reset();
   master_.reset();
   chan_.reset();
   ctrl_.reset();
@@ -141,10 +139,7 @@ void CANopenController::InitializeCANCommunication()
   master_ = std::make_shared<lely::canopen::AsyncMaster>(
     *timer_, *chan_, master_dcf_path, "", canopen_settings_.master_can_id);
 
-  front_driver_ = std::make_shared<RoboteqDriver>(
-    master_, canopen_settings_.front_driver_can_id, canopen_settings_.sdo_operation_timeout_ms);
-  rear_driver_ = std::make_shared<RoboteqDriver>(
-    master_, canopen_settings_.rear_driver_can_id, canopen_settings_.sdo_operation_timeout_ms);
+  InitializeDrivers();
 
   // Start the NMT service of the master by pretending to receive a 'reset node' command.
   master_->Reset();
@@ -159,32 +154,32 @@ void CANopenController::NotifyCANCommunicationStarted(const bool result)
   canopen_communication_started_cond_.notify_all();
 }
 
-void CANopenController::BootDrivers()
-{
-  try {
-    auto front_driver_future = front_driver_->Boot();
-    auto rear_driver_future = rear_driver_->Boot();
-
-    auto front_driver_status = front_driver_future.wait_for(std::chrono::seconds(5));
-    auto rear_driver_status = rear_driver_future.wait_for(std::chrono::seconds(5));
-
-    if (
-      front_driver_status == std::future_status::ready &&
-      rear_driver_status == std::future_status::ready) {
-      try {
-        front_driver_future.get();
-        rear_driver_future.get();
-      } catch (const std::exception & e) {
-        throw std::runtime_error("Boot failed with exception: " + std::string(e.what()));
-      }
-    } else {
-      throw std::runtime_error("Boot timed out or failed.");
-    }
-
-  } catch (const std::system_error & e) {
-    throw std::runtime_error(
-      "An exception occurred while trying to Boot driver " + std::string(e.what()));
-  }
-}
+// void CANopenController::BootDrivers()
+// {
+//   try {
+//     auto front_driver_future = front_driver_->Boot();
+//     auto rear_driver_future = rear_driver_->Boot();
+
+//     auto front_driver_status = front_driver_future.wait_for(std::chrono::seconds(5));
+//     auto rear_driver_status = rear_driver_future.wait_for(std::chrono::seconds(5));
+
+//     if (
+//       front_driver_status == std::future_status::ready &&
+//       rear_driver_status == std::future_status::ready) {
+//       try {
+//         front_driver_future.get();
+//         rear_driver_future.get();
+//       } catch (const std::exception & e) {
+//         throw std::runtime_error("Boot failed with exception: " + std::string(e.what()));
+//       }
+//     } else {
+//       throw std::runtime_error("Boot timed out or failed.");
+//     }
+
+//   } catch (const std::system_error & e) {
+//     throw std::runtime_error(
+//       "An exception occurred while trying to Boot driver " + std::string(e.what()));
+//   }
+// }
 
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp b/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp
index dc1c9a5f..667c958a 100644
--- a/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp
+++ b/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp
@@ -14,85 +14,156 @@
 
 #include <chrono>
 #include <cstdint>
+#include <future>
 #include <memory>
 #include <stdexcept>
 #include <string>
 
+#include <gmock/gmock.h>
 #include <gtest/gtest.h>
+#include <sys/socket.h>
 
 #include <panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp>
 
-#include "utils/roboteqs_mock.hpp"
 #include "utils/test_constants.hpp"
 
+class MockRoboteqDriver : public panther_hardware_interfaces::RoboteqDriverInterface
+{
+public:
+  MOCK_METHOD(std::future<void>, Boot, (), (override));
+
+  MOCK_METHOD(bool, IsCANError, (), (const, override));
+  MOCK_METHOD(bool, IsHeartbeatTimeout, (), (const, override));
+
+  MOCK_METHOD(
+    panther_hardware_interfaces::RoboteqMotorsStates, ReadRoboteqMotorsStates, (), (override));
+  MOCK_METHOD(
+    panther_hardware_interfaces::RoboteqDriverState, ReadRoboteqDriverState, (), (override));
+
+  MOCK_METHOD(
+    void, SendRoboteqCmd, (const std::int32_t cmd_channel_1, const std::int32_t cmd_channel_2),
+    (override));
+  MOCK_METHOD(void, ResetRoboteqScript, (), (override));
+  MOCK_METHOD(void, TurnOnEStop, (), (override));
+  MOCK_METHOD(void, TurnOffEStop, (), (override));
+  MOCK_METHOD(void, TurnOnSafetyStopChannel1, (), (override));
+  MOCK_METHOD(void, TurnOnSafetyStopChannel2, (), (override));
+};
+
+class MockCANopenController : public panther_hardware_interfaces::CANopenController
+{
+public:
+  MockCANopenController(const panther_hardware_interfaces::CANopenSettings & canopen_settings)
+  : CANopenController(canopen_settings)
+  {
+  }
+
+  MOCK_METHOD(void, BootDrivers, (), (override));
+  MOCK_METHOD(void, InitializeDrivers, (), (override));
+  MOCK_METHOD(
+    std::shared_ptr<panther_hardware_interfaces::RoboteqDriverInterface>, GetDriver,
+    (const std::string & name), (override));
+};
+
 class TestCANopenController : public ::testing::Test
 {
 public:
   TestCANopenController()
+  : can_interface_name_(panther_hardware_interfaces_test::kCANopenSettings.can_interface_name)
   {
-    canopen_controller_ = std::make_unique<panther_hardware_interfaces::CANopenController>(
-      panther_hardware_interfaces_test::kCANopenSettings);
+    InitializeCANSocket();
+
+    canopen_controller_ =
+      std::make_unique<MockCANopenController>(panther_hardware_interfaces_test::kCANopenSettings);
+
+    ON_CALL(*canopen_controller_, BootDrivers()).WillByDefault(testing::Return());
+    ON_CALL(*canopen_controller_, InitializeDrivers()).WillByDefault(testing::Return());
+    ON_CALL(*canopen_controller_, GetDriver(testing::_))
+      .WillByDefault(testing::Return(std::make_shared<MockRoboteqDriver>()));
+  }
+
+  ~TestCANopenController() { DeinitializeCANSocket(); }
 
-    roboteqs_mock_ = std::make_unique<panther_hardware_interfaces_test::RoboteqsMock>();
-    roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
+protected:
+  std::unique_ptr<MockCANopenController> canopen_controller_;
+
+  void InitializeCANSocket()
+  {
+    if (system("sudo modprobe vcan") != 0) {
+      throw std::runtime_error("Failed to load vcan module");
+    }
+
+    // if link already exists, do not create it
+    const auto check_command = "ip link show " + can_interface_name_ + " > /dev/null 2>&1";
+    if (std::system(check_command.c_str()) != 0) {
+      const auto command = "sudo ip link add dev " + can_interface_name_ + " type vcan";
+      if (system(command.c_str()) != 0) {
+        throw std::runtime_error("Failed to add vcan device");
+      }
+    }
+
+    const auto command = "sudo ip link set up " + can_interface_name_;
+    if (system(command.c_str()) != 0) {
+      throw std::runtime_error("Failed to set up vcan device");
+    }
   }
 
-  ~TestCANopenController()
+  void DeinitializeCANSocket()
   {
-    roboteqs_mock_->Stop();
-    roboteqs_mock_.reset();
+    std::string command = "sudo ip link set down " + can_interface_name_;
+    if (system(command.c_str()) != 0) {
+      throw std::runtime_error("Failed to delete vcan device");
+    }
   }
 
-protected:
-  std::unique_ptr<panther_hardware_interfaces_test::RoboteqsMock> roboteqs_mock_;
-  std::unique_ptr<panther_hardware_interfaces::CANopenController> canopen_controller_;
+  const std::string can_interface_name_;
 };
 
 TEST_F(TestCANopenController, InitializeAndDeinitialize)
 {
+  EXPECT_CALL(*canopen_controller_, InitializeDrivers()).Times(1);
+  EXPECT_CALL(*canopen_controller_, BootDrivers()).Times(1);
   ASSERT_NO_THROW(canopen_controller_->Initialize());
   ASSERT_NO_THROW(canopen_controller_->Deinitialize());
 
   // Check if deinitialization worked correctly - initialize once again
+  EXPECT_CALL(*canopen_controller_, InitializeDrivers()).Times(1);
+  EXPECT_CALL(*canopen_controller_, BootDrivers()).Times(1);
   ASSERT_NO_THROW(canopen_controller_->Initialize());
   ASSERT_NO_THROW(canopen_controller_->Deinitialize());
 }
 
-TEST_F(TestCANopenController, InitializeErrorDeviceType)
+TEST_F(TestCANopenController, InitializeWithError)
 {
-  roboteqs_mock_->GetFrontDriver()->SetOnReadWait<std::uint32_t>(0x1000, 0, 100000);
+  ON_CALL(*canopen_controller_, BootDrivers())
+    .WillByDefault(testing::Throw(std::runtime_error("")));
+  EXPECT_CALL(*canopen_controller_, InitializeDrivers()).Times(1);
+  EXPECT_CALL(*canopen_controller_, BootDrivers()).Times(0);
   ASSERT_THROW(canopen_controller_->Initialize(), std::runtime_error);
   ASSERT_NO_THROW(canopen_controller_->Deinitialize());
 
-  roboteqs_mock_->GetFrontDriver()->SetOnReadWait<std::uint32_t>(0x1000, 0, 0);
+  ON_CALL(*canopen_controller_, BootDrivers()).WillByDefault(testing::Return());
+  EXPECT_CALL(*canopen_controller_, InitializeDrivers()).Times(1);
+  EXPECT_CALL(*canopen_controller_, BootDrivers()).Times(1);
   ASSERT_NO_THROW(canopen_controller_->Initialize());
   ASSERT_NO_THROW(canopen_controller_->Deinitialize());
 }
 
-TEST_F(TestCANopenController, InitializeErrorVendorId)
-{
-  roboteqs_mock_->GetRearDriver()->SetOnReadWait<std::uint32_t>(0x1018, 1, 100000);
-  ASSERT_THROW(canopen_controller_->Initialize(), std::runtime_error);
-  ASSERT_NO_THROW(canopen_controller_->Deinitialize());
+// // TODO Tests for concrete implementations of CANopenController
+// TEST(TestCANopenControllerOthers, BootTimeout)
+// {
+//   std::unique_ptr<panther_hardware_interfaces::CANopenController> canopen_controller_;
 
-  roboteqs_mock_->GetRearDriver()->SetOnReadWait<std::uint32_t>(0x1018, 1, 0);
-  ASSERT_NO_THROW(canopen_controller_->Initialize());
-  ASSERT_NO_THROW(canopen_controller_->Deinitialize());
-}
-
-TEST(TestCANopenControllerOthers, BootTimeout)
-{
-  std::unique_ptr<panther_hardware_interfaces::CANopenController> canopen_controller_;
+//   canopen_controller_ = std::make_unique<panther_hardware_interfaces::CANopenController>(
+//     panther_hardware_interfaces_test::kCANopenSettings);
 
-  canopen_controller_ = std::make_unique<panther_hardware_interfaces::CANopenController>(
-    panther_hardware_interfaces_test::kCANopenSettings);
+//   // No roboteq mock, so it won't be possible to boot - here is checked if after some time it
+//   will
+//   // finish with exception
+//   ASSERT_THROW(canopen_controller_->Initialize(), std::runtime_error);
 
-  // No roboteq mock, so it won't be possible to boot - here is checked if after some time it will
-  // finish with exception
-  ASSERT_THROW(canopen_controller_->Initialize(), std::runtime_error);
-
-  canopen_controller_->Deinitialize();
-}
+//   canopen_controller_->Deinitialize();
+// }
 
 int main(int argc, char ** argv)
 {

From 89215269a9c452de4ab0f5c8fbfa88068633bab7 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Wed, 21 Aug 2024 10:13:12 +0000
Subject: [PATCH 005/100] simplify canopen controller

---
 panther_hardware_interfaces/CMakeLists.txt    |   3 +-
 .../motors_controller/canopen_controller.hpp  |  33 +----
 .../motors_controller/roboteq_driver.hpp      |   5 +
 panther_hardware_interfaces/package.xml       |   5 +-
 .../motors_controller/canopen_controller.cpp  |  33 -----
 .../test_canopen_controller.cpp               | 131 ++++++------------
 6 files changed, 53 insertions(+), 157 deletions(-)

diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt
index 17362241..37df4db8 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/panther_hardware_interfaces/CMakeLists.txt
@@ -83,7 +83,6 @@ install(
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)
-  find_package(ament_cmake_gmock REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(panther_utils REQUIRED)
 
@@ -118,7 +117,7 @@ if(BUILD_TESTING)
   target_link_libraries(${PROJECT_NAME}_test_roboteq_data_converters
                         PkgConfig::LIBLELY_COAPP)
 
-  ament_add_gmock(
+  ament_add_test(
     ${PROJECT_NAME}_test_canopen_controller
     test/panther_system/motors_controller/test_canopen_controller.cpp
     src/panther_system/motors_controller/canopen_controller.cpp
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp
index f48f7f26..53ba4292 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp
@@ -33,8 +33,6 @@
 #include "lely/io2/sys/sigset.hpp"
 #include "lely/io2/sys/timer.hpp"
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp"
-
 namespace panther_hardware_interfaces
 {
 
@@ -74,7 +72,7 @@ class CANopenController
    */
   void Deinitialize();
 
-  virtual std::shared_ptr<RoboteqDriverInterface> GetDriver(const std::string & name) = 0;
+  std::shared_ptr<lely::canopen::AsyncMaster> GetMaster() { return master_; }
 
 private:
   void InitializeCANCommunication();
@@ -87,18 +85,6 @@ class CANopenController
    */
   void NotifyCANCommunicationStarted(const bool result);
 
-  /**
-   * @brief Triggers boot on all defined Roboteq drivers and waits for finish
-   *
-   * @exception std::runtime_error if boot fails
-   */
-  virtual void BootDrivers() = 0;
-
-  /**
-   * @brief Initializes all defined Roboteq drivers
-   */
-  virtual void InitializeDrivers() = 0;
-
   // Priority set to be higher than the priority of the main ros2 control node (50)
   static constexpr unsigned kCANopenThreadSchedPriority = 60;
 
@@ -122,23 +108,6 @@ class CANopenController
   const CANopenSettings canopen_settings_;
 };
 
-// class PantherCANopenController : public CANopenController
-// {
-// public:
-//   PantherCANopenController(const CANopenSettings & canopen_settings);
-
-//   std::shared_ptr<RoboteqDriverInterface> GetDriver(const std::string & name) override;
-
-// private:
-//   void BootDrivers() override;
-//   void InitializeDrivers() override;
-
-//   std::shared_ptr<RoboteqDriver> front_driver_;
-//   std::shared_ptr<RoboteqDriver> rear_driver_;
-
-//   void ResetDrivers();
-// };
-
 }  // namespace panther_hardware_interfaces
 
 #endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_CONTROLLER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
index 081201e1..1fe94746 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
@@ -137,6 +137,11 @@ class RoboteqDriverInterface
    * @exception std::runtime_error if any operation returns error
    */
   virtual void TurnOnSafetyStopChannel2() = 0;
+
+    /**
+   * @brief Alias for a shared pointer to a RoboteqDriverInterface object.
+   */
+  using SharedPtr = std::shared_ptr<RoboteqDriverInterface>;
 };
 
 /**
diff --git a/panther_hardware_interfaces/package.xml b/panther_hardware_interfaces/package.xml
index f3a90b58..df075d21 100644
--- a/panther_hardware_interfaces/package.xml
+++ b/panther_hardware_interfaces/package.xml
@@ -11,10 +11,11 @@
   <url type="repository">https://github.com/husarion/panther_ros</url>
   <url type="bugtracker">https://github.com/husarion/panther_ros/issues</url>
 
-  <author email="maciej.stepien@husarion.com">Maciej Stępień</author>
-  <author email="pawel.kowalski@husarion.com">Paweł Kowalski</author>
   <author email="jakub.delicat@husarion.com">Jakub Delicat</author>
   <author email="pawel.irzyk@husarion.com">Paweł Irzyk</author>
+  <author email="dawid.kmak@husarion.com">Dawid Kmak</author>
+  <author email="pawel.kowalski@husarion.com">Paweł Kowalski</author>
+  <author email="maciej.stepien@husarion.com">Maciej Stępień</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp b/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp
index eb594d8d..50cfc9d4 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp
+++ b/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp
@@ -16,7 +16,6 @@
 
 #include <condition_variable>
 #include <filesystem>
-#include <future>
 #include <iostream>
 #include <memory>
 #include <mutex>
@@ -81,8 +80,6 @@ void CANopenController::Initialize()
     throw std::runtime_error("CAN communication not initialized.");
   }
 
-  BootDrivers();
-
   initialized_ = true;
 }
 
@@ -139,8 +136,6 @@ void CANopenController::InitializeCANCommunication()
   master_ = std::make_shared<lely::canopen::AsyncMaster>(
     *timer_, *chan_, master_dcf_path, "", canopen_settings_.master_can_id);
 
-  InitializeDrivers();
-
   // Start the NMT service of the master by pretending to receive a 'reset node' command.
   master_->Reset();
 }
@@ -154,32 +149,4 @@ void CANopenController::NotifyCANCommunicationStarted(const bool result)
   canopen_communication_started_cond_.notify_all();
 }
 
-// void CANopenController::BootDrivers()
-// {
-//   try {
-//     auto front_driver_future = front_driver_->Boot();
-//     auto rear_driver_future = rear_driver_->Boot();
-
-//     auto front_driver_status = front_driver_future.wait_for(std::chrono::seconds(5));
-//     auto rear_driver_status = rear_driver_future.wait_for(std::chrono::seconds(5));
-
-//     if (
-//       front_driver_status == std::future_status::ready &&
-//       rear_driver_status == std::future_status::ready) {
-//       try {
-//         front_driver_future.get();
-//         rear_driver_future.get();
-//       } catch (const std::exception & e) {
-//         throw std::runtime_error("Boot failed with exception: " + std::string(e.what()));
-//       }
-//     } else {
-//       throw std::runtime_error("Boot timed out or failed.");
-//     }
-
-//   } catch (const std::system_error & e) {
-//     throw std::runtime_error(
-//       "An exception occurred while trying to Boot driver " + std::string(e.what()));
-//   }
-// }
-
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp b/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp
index 667c958a..a97e4d44 100644
--- a/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp
+++ b/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp
@@ -12,14 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <chrono>
-#include <cstdint>
-#include <future>
 #include <memory>
 #include <stdexcept>
 #include <string>
 
-#include <gmock/gmock.h>
 #include <gtest/gtest.h>
 #include <sys/socket.h>
 
@@ -27,68 +23,19 @@
 
 #include "utils/test_constants.hpp"
 
-class MockRoboteqDriver : public panther_hardware_interfaces::RoboteqDriverInterface
+class FakeCANSocket
 {
 public:
-  MOCK_METHOD(std::future<void>, Boot, (), (override));
-
-  MOCK_METHOD(bool, IsCANError, (), (const, override));
-  MOCK_METHOD(bool, IsHeartbeatTimeout, (), (const, override));
-
-  MOCK_METHOD(
-    panther_hardware_interfaces::RoboteqMotorsStates, ReadRoboteqMotorsStates, (), (override));
-  MOCK_METHOD(
-    panther_hardware_interfaces::RoboteqDriverState, ReadRoboteqDriverState, (), (override));
-
-  MOCK_METHOD(
-    void, SendRoboteqCmd, (const std::int32_t cmd_channel_1, const std::int32_t cmd_channel_2),
-    (override));
-  MOCK_METHOD(void, ResetRoboteqScript, (), (override));
-  MOCK_METHOD(void, TurnOnEStop, (), (override));
-  MOCK_METHOD(void, TurnOffEStop, (), (override));
-  MOCK_METHOD(void, TurnOnSafetyStopChannel1, (), (override));
-  MOCK_METHOD(void, TurnOnSafetyStopChannel2, (), (override));
-};
-
-class MockCANopenController : public panther_hardware_interfaces::CANopenController
-{
-public:
-  MockCANopenController(const panther_hardware_interfaces::CANopenSettings & canopen_settings)
-  : CANopenController(canopen_settings)
+  FakeCANSocket(const std::string & can_interface_name)
+  : can_interface_name_(can_interface_name), can_device_created_(false)
   {
   }
 
-  MOCK_METHOD(void, BootDrivers, (), (override));
-  MOCK_METHOD(void, InitializeDrivers, (), (override));
-  MOCK_METHOD(
-    std::shared_ptr<panther_hardware_interfaces::RoboteqDriverInterface>, GetDriver,
-    (const std::string & name), (override));
-};
+  ~FakeCANSocket() { Deinitialize(); }
 
-class TestCANopenController : public ::testing::Test
-{
-public:
-  TestCANopenController()
-  : can_interface_name_(panther_hardware_interfaces_test::kCANopenSettings.can_interface_name)
-  {
-    InitializeCANSocket();
-
-    canopen_controller_ =
-      std::make_unique<MockCANopenController>(panther_hardware_interfaces_test::kCANopenSettings);
-
-    ON_CALL(*canopen_controller_, BootDrivers()).WillByDefault(testing::Return());
-    ON_CALL(*canopen_controller_, InitializeDrivers()).WillByDefault(testing::Return());
-    ON_CALL(*canopen_controller_, GetDriver(testing::_))
-      .WillByDefault(testing::Return(std::make_shared<MockRoboteqDriver>()));
-  }
-
-  ~TestCANopenController() { DeinitializeCANSocket(); }
-
-protected:
-  std::unique_ptr<MockCANopenController> canopen_controller_;
-
-  void InitializeCANSocket()
+  void Initialize()
   {
+    std::cout << "Initializing vcan device" << std::endl;
     if (system("sudo modprobe vcan") != 0) {
       throw std::runtime_error("Failed to load vcan module");
     }
@@ -102,69 +49,77 @@ class TestCANopenController : public ::testing::Test
       }
     }
 
+    can_device_created_ = true;
+
     const auto command = "sudo ip link set up " + can_interface_name_;
     if (system(command.c_str()) != 0) {
       throw std::runtime_error("Failed to set up vcan device");
     }
   }
 
-  void DeinitializeCANSocket()
+  void Deinitialize()
   {
-    std::string command = "sudo ip link set down " + can_interface_name_;
+    if (!can_device_created_) {
+      return;
+    }
+
+    std::string command = "sudo ip link delete " + can_interface_name_;
     if (system(command.c_str()) != 0) {
       throw std::runtime_error("Failed to delete vcan device");
     }
+
+    can_device_created_ = false;
   }
 
+private:
   const std::string can_interface_name_;
+  bool can_device_created_;
+};
+
+class TestCANopenController : public ::testing::Test
+{
+public:
+  TestCANopenController();
+
+  ~TestCANopenController() {}
+
+protected:
+  std::unique_ptr<FakeCANSocket> can_socket_;
+  std::unique_ptr<panther_hardware_interfaces::CANopenController> canopen_controller_;
 };
 
+TestCANopenController::TestCANopenController()
+{
+  can_socket_ = std::make_unique<FakeCANSocket>(
+    panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
+
+  canopen_controller_ = std::make_unique<panther_hardware_interfaces::CANopenController>(
+    panther_hardware_interfaces_test::kCANopenSettings);
+}
+
 TEST_F(TestCANopenController, InitializeAndDeinitialize)
 {
-  EXPECT_CALL(*canopen_controller_, InitializeDrivers()).Times(1);
-  EXPECT_CALL(*canopen_controller_, BootDrivers()).Times(1);
+  can_socket_->Initialize();
+
   ASSERT_NO_THROW(canopen_controller_->Initialize());
   ASSERT_NO_THROW(canopen_controller_->Deinitialize());
 
   // Check if deinitialization worked correctly - initialize once again
-  EXPECT_CALL(*canopen_controller_, InitializeDrivers()).Times(1);
-  EXPECT_CALL(*canopen_controller_, BootDrivers()).Times(1);
   ASSERT_NO_THROW(canopen_controller_->Initialize());
   ASSERT_NO_THROW(canopen_controller_->Deinitialize());
 }
 
 TEST_F(TestCANopenController, InitializeWithError)
 {
-  ON_CALL(*canopen_controller_, BootDrivers())
-    .WillByDefault(testing::Throw(std::runtime_error("")));
-  EXPECT_CALL(*canopen_controller_, InitializeDrivers()).Times(1);
-  EXPECT_CALL(*canopen_controller_, BootDrivers()).Times(0);
+  // CAN socket not initialized, should throw
   ASSERT_THROW(canopen_controller_->Initialize(), std::runtime_error);
   ASSERT_NO_THROW(canopen_controller_->Deinitialize());
 
-  ON_CALL(*canopen_controller_, BootDrivers()).WillByDefault(testing::Return());
-  EXPECT_CALL(*canopen_controller_, InitializeDrivers()).Times(1);
-  EXPECT_CALL(*canopen_controller_, BootDrivers()).Times(1);
+  can_socket_->Initialize();
   ASSERT_NO_THROW(canopen_controller_->Initialize());
   ASSERT_NO_THROW(canopen_controller_->Deinitialize());
 }
 
-// // TODO Tests for concrete implementations of CANopenController
-// TEST(TestCANopenControllerOthers, BootTimeout)
-// {
-//   std::unique_ptr<panther_hardware_interfaces::CANopenController> canopen_controller_;
-
-//   canopen_controller_ = std::make_unique<panther_hardware_interfaces::CANopenController>(
-//     panther_hardware_interfaces_test::kCANopenSettings);
-
-//   // No roboteq mock, so it won't be possible to boot - here is checked if after some time it
-//   will
-//   // finish with exception
-//   ASSERT_THROW(canopen_controller_->Initialize(), std::runtime_error);
-
-//   canopen_controller_->Deinitialize();
-// }
-
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);

From f7fae9d273e810d1324f7583c654b0464bc1367c Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Wed, 21 Aug 2024 10:14:31 +0000
Subject: [PATCH 006/100] fix make

---
 panther_hardware_interfaces/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt
index 37df4db8..8713f699 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/panther_hardware_interfaces/CMakeLists.txt
@@ -117,7 +117,7 @@ if(BUILD_TESTING)
   target_link_libraries(${PROJECT_NAME}_test_roboteq_data_converters
                         PkgConfig::LIBLELY_COAPP)
 
-  ament_add_test(
+  ament_add_gtest(
     ${PROJECT_NAME}_test_canopen_controller
     test/panther_system/motors_controller/test_canopen_controller.cpp
     src/panther_system/motors_controller/canopen_controller.cpp

From 2162292602bd547e58336ce9dbc339d00b4405f5 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Wed, 21 Aug 2024 11:13:51 +0000
Subject: [PATCH 007/100] update roboteq_driver tests

---
 .../test_canopen_controller.cpp               |  58 +--
 .../motors_controller/test_roboteq_driver.cpp | 343 +++++++-----------
 .../test/utils/fake_can_socket.hpp            |  80 ++++
 .../test/utils/roboteqs_mock.hpp              |  43 +--
 4 files changed, 220 insertions(+), 304 deletions(-)
 create mode 100644 panther_hardware_interfaces/test/utils/fake_can_socket.hpp

diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp b/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp
index a97e4d44..e1833be8 100644
--- a/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp
+++ b/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp
@@ -21,61 +21,9 @@
 
 #include <panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp>
 
+#include "utils/fake_can_socket.hpp"
 #include "utils/test_constants.hpp"
 
-class FakeCANSocket
-{
-public:
-  FakeCANSocket(const std::string & can_interface_name)
-  : can_interface_name_(can_interface_name), can_device_created_(false)
-  {
-  }
-
-  ~FakeCANSocket() { Deinitialize(); }
-
-  void Initialize()
-  {
-    std::cout << "Initializing vcan device" << std::endl;
-    if (system("sudo modprobe vcan") != 0) {
-      throw std::runtime_error("Failed to load vcan module");
-    }
-
-    // if link already exists, do not create it
-    const auto check_command = "ip link show " + can_interface_name_ + " > /dev/null 2>&1";
-    if (std::system(check_command.c_str()) != 0) {
-      const auto command = "sudo ip link add dev " + can_interface_name_ + " type vcan";
-      if (system(command.c_str()) != 0) {
-        throw std::runtime_error("Failed to add vcan device");
-      }
-    }
-
-    can_device_created_ = true;
-
-    const auto command = "sudo ip link set up " + can_interface_name_;
-    if (system(command.c_str()) != 0) {
-      throw std::runtime_error("Failed to set up vcan device");
-    }
-  }
-
-  void Deinitialize()
-  {
-    if (!can_device_created_) {
-      return;
-    }
-
-    std::string command = "sudo ip link delete " + can_interface_name_;
-    if (system(command.c_str()) != 0) {
-      throw std::runtime_error("Failed to delete vcan device");
-    }
-
-    can_device_created_ = false;
-  }
-
-private:
-  const std::string can_interface_name_;
-  bool can_device_created_;
-};
-
 class TestCANopenController : public ::testing::Test
 {
 public:
@@ -84,13 +32,13 @@ class TestCANopenController : public ::testing::Test
   ~TestCANopenController() {}
 
 protected:
-  std::unique_ptr<FakeCANSocket> can_socket_;
+  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
   std::unique_ptr<panther_hardware_interfaces::CANopenController> canopen_controller_;
 };
 
 TestCANopenController::TestCANopenController()
 {
-  can_socket_ = std::make_unique<FakeCANSocket>(
+  can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
     panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
 
   canopen_controller_ = std::make_unique<panther_hardware_interfaces::CANopenController>(
diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_driver.cpp
index 7f83e88d..1a4463d0 100644
--- a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_driver.cpp
+++ b/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_driver.cpp
@@ -20,9 +20,10 @@
 
 #include <gtest/gtest.h>
 
-#include <panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp>
-#include <panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp>
+#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp"
+#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp"
 
+#include "utils/fake_can_socket.hpp"
 #include "utils/roboteqs_mock.hpp"
 #include "utils/test_constants.hpp"
 
@@ -33,131 +34,103 @@ class TestRoboteqDriver : public ::testing::Test
 public:
   TestRoboteqDriver()
   {
+    can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
+      panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
+    can_socket_->Initialize();
+
     canopen_controller_ = std::make_unique<panther_hardware_interfaces::CANopenController>(
       panther_hardware_interfaces_test::kCANopenSettings);
 
     roboteqs_mock_ = std::make_unique<panther_hardware_interfaces_test::RoboteqsMock>();
     roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
     canopen_controller_->Initialize();
+
+    roboteq_driver_ = std::make_shared<panther_hardware_interfaces::RoboteqDriver>(
+      canopen_controller_->GetMaster(), 1, std::chrono::milliseconds(100));
+    roboteq_driver_->Boot();
   }
 
   ~TestRoboteqDriver()
   {
+    roboteq_driver_.reset();
     canopen_controller_->Deinitialize();
     roboteqs_mock_->Stop();
     roboteqs_mock_.reset();
+    can_socket_->Deinitialize();
   }
 
 protected:
+  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
   std::unique_ptr<panther_hardware_interfaces_test::RoboteqsMock> roboteqs_mock_;
   std::unique_ptr<panther_hardware_interfaces::CANopenController> canopen_controller_;
+  panther_hardware_interfaces::RoboteqDriverInterface::SharedPtr roboteq_driver_;
 };
 
 TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
 {
   using panther_hardware_interfaces_test::DriverChannel;
 
-  const std::int32_t fl_pos = 101;
-  const std::int32_t fl_vel = 102;
-  const std::int32_t fl_current = 103;
-  const std::int32_t fr_pos = 201;
-  const std::int32_t fr_vel = 202;
-  const std::int32_t fr_current = 203;
-  const std::int32_t rl_pos = 301;
-  const std::int32_t rl_vel = 302;
-  const std::int32_t rl_current = 303;
-  const std::int32_t rr_pos = 401;
-  const std::int32_t rr_vel = 402;
-  const std::int32_t rr_current = 403;
-
-  roboteqs_mock_->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL2, fl_pos);
-  roboteqs_mock_->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL1, fr_pos);
-  roboteqs_mock_->GetRearDriver()->SetPosition(DriverChannel::CHANNEL2, rl_pos);
-  roboteqs_mock_->GetRearDriver()->SetPosition(DriverChannel::CHANNEL1, rr_pos);
-
-  roboteqs_mock_->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL2, fl_vel);
-  roboteqs_mock_->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL1, fr_vel);
-  roboteqs_mock_->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL2, rl_vel);
-  roboteqs_mock_->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL1, rr_vel);
-
-  roboteqs_mock_->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL2, fl_current);
-  roboteqs_mock_->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL1, fr_current);
-  roboteqs_mock_->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL2, rl_current);
-  roboteqs_mock_->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL1, rr_current);
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-
-  panther_hardware_interfaces::RoboteqMotorsStates f_fb =
-    canopen_controller_->GetFrontDriver()->ReadRoboteqMotorsStates();
-  panther_hardware_interfaces::RoboteqMotorsStates r_fb =
-    canopen_controller_->GetRearDriver()->ReadRoboteqMotorsStates();
-
-  EXPECT_EQ(f_fb.motor_2.pos, fl_pos);
-  EXPECT_EQ(f_fb.motor_2.vel, fl_vel);
-  EXPECT_EQ(f_fb.motor_2.current, fl_current);
-
-  EXPECT_EQ(f_fb.motor_1.pos, fr_pos);
-  EXPECT_EQ(f_fb.motor_1.vel, fr_vel);
-  EXPECT_EQ(f_fb.motor_1.current, fr_current);
-
-  EXPECT_EQ(r_fb.motor_2.pos, rl_pos);
-  EXPECT_EQ(r_fb.motor_2.vel, rl_vel);
-  EXPECT_EQ(r_fb.motor_2.current, rl_current);
-
-  EXPECT_EQ(r_fb.motor_1.pos, rr_pos);
-  EXPECT_EQ(r_fb.motor_1.vel, rr_vel);
-  EXPECT_EQ(r_fb.motor_1.current, rr_current);
+  const std::int32_t left_pos = 101;
+  const std::int32_t left_vel = 102;
+  const std::int32_t left_current = 103;
+  const std::int32_t right_pos = 201;
+  const std::int32_t right_vel = 202;
+  const std::int32_t right_current = 203;
+
+  roboteqs_mock_->GetDriver()->SetPosition(DriverChannel::CHANNEL2, left_pos);
+  roboteqs_mock_->GetDriver()->SetPosition(DriverChannel::CHANNEL1, right_pos);
+
+  roboteqs_mock_->GetDriver()->SetVelocity(DriverChannel::CHANNEL2, left_vel);
+  roboteqs_mock_->GetDriver()->SetVelocity(DriverChannel::CHANNEL1, right_vel);
+
+  roboteqs_mock_->GetDriver()->SetCurrent(DriverChannel::CHANNEL2, left_current);
+  roboteqs_mock_->GetDriver()->SetCurrent(DriverChannel::CHANNEL1, right_current);
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+  panther_hardware_interfaces::RoboteqMotorsStates fb =
+    roboteq_driver_->ReadRoboteqMotorsStates();
+
+  EXPECT_EQ(fb.motor_2.pos, left_pos);
+  EXPECT_EQ(fb.motor_2.vel, left_vel);
+  EXPECT_EQ(fb.motor_2.current, left_current);
+
+  EXPECT_EQ(fb.motor_1.pos, right_pos);
+  EXPECT_EQ(fb.motor_1.vel, right_vel);
+  EXPECT_EQ(fb.motor_1.current, right_current);
 }
 
 TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps)
 {
   std::this_thread::sleep_for(std::chrono::milliseconds(150));
 
-  panther_hardware_interfaces::RoboteqMotorsStates f_fb1 =
-    canopen_controller_->GetFrontDriver()->ReadRoboteqMotorsStates();
-  panther_hardware_interfaces::RoboteqMotorsStates r_fb1 =
-    canopen_controller_->GetRearDriver()->ReadRoboteqMotorsStates();
+  panther_hardware_interfaces::RoboteqMotorsStates fb1 =
+    roboteq_driver_->ReadRoboteqMotorsStates();
 
   // based on publishing frequency in the Roboteq mock (10)
   std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
-  panther_hardware_interfaces::RoboteqMotorsStates f_fb2 =
-    canopen_controller_->GetFrontDriver()->ReadRoboteqMotorsStates();
-  panther_hardware_interfaces::RoboteqMotorsStates r_fb2 =
-    canopen_controller_->GetRearDriver()->ReadRoboteqMotorsStates();
+  panther_hardware_interfaces::RoboteqMotorsStates fb2 =
+    roboteq_driver_->ReadRoboteqMotorsStates();
 
   // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
   // if consecutive messages will have timestamps 100ms + some threshold apart
   EXPECT_LE(
-    lely::util::from_timespec(f_fb2.pos_timestamp) - lely::util::from_timespec(f_fb1.pos_timestamp),
-    std::chrono::milliseconds(15));
-  EXPECT_LE(
-    lely::util::from_timespec(r_fb2.pos_timestamp) - lely::util::from_timespec(r_fb1.pos_timestamp),
+    lely::util::from_timespec(fb2.pos_timestamp) - lely::util::from_timespec(fb1.pos_timestamp),
     std::chrono::milliseconds(15));
 
   EXPECT_GE(
-    lely::util::from_timespec(f_fb2.pos_timestamp) - lely::util::from_timespec(f_fb1.pos_timestamp),
-    std::chrono::milliseconds(5));
-  EXPECT_GE(
-    lely::util::from_timespec(r_fb2.pos_timestamp) - lely::util::from_timespec(r_fb1.pos_timestamp),
+    lely::util::from_timespec(fb2.pos_timestamp) - lely::util::from_timespec(fb1.pos_timestamp),
     std::chrono::milliseconds(5));
 
   EXPECT_LE(
-    lely::util::from_timespec(f_fb2.vel_current_timestamp) -
-      lely::util::from_timespec(f_fb1.vel_current_timestamp),
-    std::chrono::milliseconds(15));
-  EXPECT_LE(
-    lely::util::from_timespec(r_fb2.vel_current_timestamp) -
-      lely::util::from_timespec(r_fb1.vel_current_timestamp),
+    lely::util::from_timespec(fb2.vel_current_timestamp) -
+      lely::util::from_timespec(fb1.vel_current_timestamp),
     std::chrono::milliseconds(15));
 
   EXPECT_GE(
-    lely::util::from_timespec(f_fb2.vel_current_timestamp) -
-      lely::util::from_timespec(f_fb1.vel_current_timestamp),
-    std::chrono::milliseconds(5));
-  EXPECT_GE(
-    lely::util::from_timespec(r_fb2.vel_current_timestamp) -
-      lely::util::from_timespec(r_fb1.vel_current_timestamp),
+    lely::util::from_timespec(fb2.vel_current_timestamp) -
+      lely::util::from_timespec(fb1.vel_current_timestamp),
     std::chrono::milliseconds(5));
 }
 
@@ -168,128 +141,75 @@ TEST_F(TestRoboteqDriver, ReadRoboteqDriverState)
   using panther_hardware_interfaces_test::DriverRuntimeErrors;
   using panther_hardware_interfaces_test::DriverScriptFlags;
 
-  const std::int16_t f_temp = 30;
-  const std::int16_t r_temp = 32;
-  const std::int16_t f_heatsink_temp = 31;
-  const std::int16_t r_heatsink_temp = 33;
-  const std::uint16_t f_volt = 400;
-  const std::uint16_t r_volt = 430;
-  const std::int16_t f_battery_current_1 = 10;
-  const std::int16_t r_battery_current_1 = 30;
-  const std::int16_t f_battery_current_2 = 30;
-  const std::int16_t r_battery_current_2 = 40;
-
-  roboteqs_mock_->GetFrontDriver()->SetTemperature(f_temp);
-  roboteqs_mock_->GetRearDriver()->SetTemperature(r_temp);
-  roboteqs_mock_->GetFrontDriver()->SetHeatsinkTemperature(f_heatsink_temp);
-  roboteqs_mock_->GetRearDriver()->SetHeatsinkTemperature(r_heatsink_temp);
-  roboteqs_mock_->GetFrontDriver()->SetVoltage(f_volt);
-  roboteqs_mock_->GetRearDriver()->SetVoltage(r_volt);
-  roboteqs_mock_->GetFrontDriver()->SetBatteryCurrent1(f_battery_current_1);
-  roboteqs_mock_->GetRearDriver()->SetBatteryCurrent1(r_battery_current_1);
-  roboteqs_mock_->GetFrontDriver()->SetBatteryCurrent2(f_battery_current_2);
-  roboteqs_mock_->GetRearDriver()->SetBatteryCurrent2(r_battery_current_2);
-
-  roboteqs_mock_->GetFrontDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERHEAT);
-  roboteqs_mock_->GetFrontDriver()->SetDriverScriptFlag(DriverScriptFlags::ENCODER_DISCONNECTED);
-  roboteqs_mock_->GetFrontDriver()->SetDriverRuntimeError(
+  const std::int16_t temp = 30;
+  const std::int16_t heatsink_temp = 31;
+  const std::uint16_t volt = 400;
+  const std::int16_t battery_current_1 = 10;
+  const std::int16_t battery_current_2 = 30;
+
+  roboteqs_mock_->GetDriver()->SetTemperature(temp);
+  roboteqs_mock_->GetDriver()->SetHeatsinkTemperature(heatsink_temp);
+  roboteqs_mock_->GetDriver()->SetVoltage(volt);
+  roboteqs_mock_->GetDriver()->SetBatteryCurrent1(battery_current_1);
+  roboteqs_mock_->GetDriver()->SetBatteryCurrent2(battery_current_2);
+
+  roboteqs_mock_->GetDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERHEAT);
+  roboteqs_mock_->GetDriver()->SetDriverScriptFlag(DriverScriptFlags::ENCODER_DISCONNECTED);
+  roboteqs_mock_->GetDriver()->SetDriverRuntimeError(
     DriverChannel::CHANNEL1, DriverRuntimeErrors::LOOP_ERROR);
-  roboteqs_mock_->GetFrontDriver()->SetDriverRuntimeError(
+  roboteqs_mock_->GetDriver()->SetDriverRuntimeError(
     DriverChannel::CHANNEL2, DriverRuntimeErrors::SAFETY_STOP_ACTIVE);
 
-  roboteqs_mock_->GetRearDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERVOLTAGE);
-  roboteqs_mock_->GetRearDriver()->SetDriverScriptFlag(DriverScriptFlags::AMP_LIMITER);
-  roboteqs_mock_->GetRearDriver()->SetDriverRuntimeError(
-    DriverChannel::CHANNEL1, DriverRuntimeErrors::FORWARD_LIMIT_TRIGGERED);
-  roboteqs_mock_->GetRearDriver()->SetDriverRuntimeError(
-    DriverChannel::CHANNEL2, DriverRuntimeErrors::REVERSE_LIMIT_TRIGGERED);
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-
-  panther_hardware_interfaces::RoboteqDriverState f_fb =
-    canopen_controller_->GetFrontDriver()->ReadRoboteqDriverState();
-  panther_hardware_interfaces::RoboteqDriverState r_fb =
-    canopen_controller_->GetRearDriver()->ReadRoboteqDriverState();
-
-  EXPECT_EQ(f_fb.mcu_temp, f_temp);
-  EXPECT_EQ(r_fb.mcu_temp, r_temp);
-
-  EXPECT_EQ(f_fb.heatsink_temp, f_heatsink_temp);
-  EXPECT_EQ(r_fb.heatsink_temp, r_heatsink_temp);
-
-  EXPECT_EQ(f_fb.battery_voltage, f_volt);
-  EXPECT_EQ(r_fb.battery_voltage, r_volt);
-
-  EXPECT_EQ(f_fb.battery_current_1, f_battery_current_1);
-  EXPECT_EQ(r_fb.battery_current_1, r_battery_current_1);
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  EXPECT_EQ(f_fb.battery_current_2, f_battery_current_2);
-  EXPECT_EQ(r_fb.battery_current_2, r_battery_current_2);
+  panther_hardware_interfaces::RoboteqDriverState fb =
+    roboteq_driver_->ReadRoboteqDriverState();
 
-  EXPECT_EQ(f_fb.fault_flags, 0b00000001);
-  EXPECT_EQ(f_fb.script_flags, 0b00000010);
-  EXPECT_EQ(f_fb.runtime_stat_flag_motor_1, 0b00000100);
-  EXPECT_EQ(f_fb.runtime_stat_flag_motor_2, 0b00001000);
+  EXPECT_EQ(fb.mcu_temp, temp);
+  EXPECT_EQ(fb.heatsink_temp, heatsink_temp);
+  EXPECT_EQ(fb.battery_voltage, volt);
+  EXPECT_EQ(fb.battery_current_1, battery_current_1);
+  EXPECT_EQ(fb.battery_current_2, battery_current_2);
 
-  EXPECT_EQ(r_fb.fault_flags, 0b00000010);
-  EXPECT_EQ(r_fb.script_flags, 0b00000100);
-  EXPECT_EQ(r_fb.runtime_stat_flag_motor_1, 0b00010000);
-  EXPECT_EQ(r_fb.runtime_stat_flag_motor_2, 0b00100000);
+  EXPECT_EQ(fb.fault_flags, 0b00000001);
+  EXPECT_EQ(fb.script_flags, 0b00000010);
+  EXPECT_EQ(fb.runtime_stat_flag_motor_1, 0b00000100);
+  EXPECT_EQ(fb.runtime_stat_flag_motor_2, 0b00001000);
 }
 
 TEST_F(TestRoboteqDriver, ReadRoboteqDriverStateTimestamp)
 {
   std::this_thread::sleep_for(std::chrono::milliseconds(150));
 
-  panther_hardware_interfaces::RoboteqDriverState f_fb1 =
-    canopen_controller_->GetFrontDriver()->ReadRoboteqDriverState();
-  panther_hardware_interfaces::RoboteqDriverState r_fb1 =
-    canopen_controller_->GetRearDriver()->ReadRoboteqDriverState();
+  panther_hardware_interfaces::RoboteqDriverState fb1 =
+    roboteq_driver_->ReadRoboteqDriverState();
 
   // based on publishing frequency in the Roboteq mock (50)
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  panther_hardware_interfaces::RoboteqDriverState f_fb2 =
-    canopen_controller_->GetFrontDriver()->ReadRoboteqDriverState();
-  panther_hardware_interfaces::RoboteqDriverState r_fb2 =
-    canopen_controller_->GetRearDriver()->ReadRoboteqDriverState();
+  panther_hardware_interfaces::RoboteqDriverState fb2 =
+    roboteq_driver_->ReadRoboteqDriverState();
 
   // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
   // if consecutive messages will have timestamps 100ms + some threshold apart
   EXPECT_LE(
-    lely::util::from_timespec(f_fb2.flags_current_timestamp) -
-      lely::util::from_timespec(f_fb1.flags_current_timestamp),
-    std::chrono::milliseconds(75));
-  EXPECT_LE(
-    lely::util::from_timespec(r_fb2.flags_current_timestamp) -
-      lely::util::from_timespec(r_fb1.flags_current_timestamp),
+    lely::util::from_timespec(fb2.flags_current_timestamp) -
+      lely::util::from_timespec(fb1.flags_current_timestamp),
     std::chrono::milliseconds(75));
 
   EXPECT_GE(
-    lely::util::from_timespec(f_fb2.flags_current_timestamp) -
-      lely::util::from_timespec(f_fb1.flags_current_timestamp),
-    std::chrono::milliseconds(25));
-  EXPECT_GE(
-    lely::util::from_timespec(r_fb2.flags_current_timestamp) -
-      lely::util::from_timespec(r_fb1.flags_current_timestamp),
+    lely::util::from_timespec(fb2.flags_current_timestamp) -
+      lely::util::from_timespec(fb1.flags_current_timestamp),
     std::chrono::milliseconds(25));
 
   EXPECT_LE(
-    lely::util::from_timespec(f_fb2.voltages_temps_timestamp) -
-      lely::util::from_timespec(f_fb1.voltages_temps_timestamp),
-    std::chrono::milliseconds(75));
-  EXPECT_LE(
-    lely::util::from_timespec(r_fb2.voltages_temps_timestamp) -
-      lely::util::from_timespec(r_fb1.voltages_temps_timestamp),
+    lely::util::from_timespec(fb2.voltages_temps_timestamp) -
+      lely::util::from_timespec(fb1.voltages_temps_timestamp),
     std::chrono::milliseconds(75));
 
   EXPECT_GE(
-    lely::util::from_timespec(f_fb2.voltages_temps_timestamp) -
-      lely::util::from_timespec(f_fb1.voltages_temps_timestamp),
-    std::chrono::milliseconds(25));
-  EXPECT_GE(
-    lely::util::from_timespec(r_fb2.voltages_temps_timestamp) -
-      lely::util::from_timespec(r_fb1.voltages_temps_timestamp),
+    lely::util::from_timespec(fb2.voltages_temps_timestamp) -
+      lely::util::from_timespec(fb1.voltages_temps_timestamp),
     std::chrono::milliseconds(25));
 }
 
@@ -297,87 +217,68 @@ TEST_F(TestRoboteqDriver, SendRoboteqCmd)
 {
   using panther_hardware_interfaces_test::DriverChannel;
 
-  const std::int32_t fl_v = 10;
-  const std::int32_t fr_v = 20;
-  const std::int32_t rl_v = 30;
-  const std::int32_t rr_v = 40;
+  const std::int32_t left_v = 10;
+  const std::int32_t right_v = 20;
 
-  canopen_controller_->GetFrontDriver()->SendRoboteqCmd(fr_v, fl_v);
-  canopen_controller_->GetRearDriver()->SendRoboteqCmd(rr_v, rl_v);
+  roboteq_driver_->SendRoboteqCmd(right_v, left_v);
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), fl_v);
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), fr_v);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), rl_v);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), rr_v);
+  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), left_v);
+  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), right_v);
 }
 
 TEST_F(TestRoboteqDriver, ResetRoboteqScript)
 {
-  roboteqs_mock_->GetFrontDriver()->SetResetRoboteqScript(65);
-  roboteqs_mock_->GetRearDriver()->SetResetRoboteqScript(23);
-
-  canopen_controller_->GetFrontDriver()->ResetRoboteqScript();
-  canopen_controller_->GetRearDriver()->ResetRoboteqScript();
+  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  roboteqs_mock_->GetDriver()->SetResetRoboteqScript(65);
+  roboteq_driver_->ResetRoboteqScript();
 
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetResetRoboteqScript(), 2);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetResetRoboteqScript(), 2);
+  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetResetRoboteqScript(), 2);
 }
 
 TEST_F(TestRoboteqDriver, ReadRoboteqTurnOnEStop)
 {
-  roboteqs_mock_->GetFrontDriver()->SetTurnOnEStop(65);
-  roboteqs_mock_->GetRearDriver()->SetTurnOnEStop(23);
-
-  canopen_controller_->GetFrontDriver()->TurnOnEStop();
-  canopen_controller_->GetRearDriver()->TurnOnEStop();
+  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  roboteqs_mock_->GetDriver()->SetTurnOnEStop(65);
+  roboteq_driver_->TurnOnEStop();
 
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOnEStop(), 1);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOnEStop(), 1);
+  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetTurnOnEStop(), 1);
 }
 
 TEST_F(TestRoboteqDriver, TurnOffEStop)
 {
-  roboteqs_mock_->GetFrontDriver()->SetTurnOffEStop(65);
-  roboteqs_mock_->GetRearDriver()->SetTurnOffEStop(23);
-
-  canopen_controller_->GetFrontDriver()->TurnOffEStop();
-  canopen_controller_->GetRearDriver()->TurnOffEStop();
+  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  roboteqs_mock_->GetDriver()->SetTurnOffEStop(65);
+  roboteq_driver_->TurnOffEStop();
 
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOffEStop(), 1);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOffEStop(), 1);
+  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetTurnOffEStop(), 1);
 }
 
 TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel1)
 {
-  roboteqs_mock_->GetFrontDriver()->SetTurnOnSafetyStop(67);
-  roboteqs_mock_->GetRearDriver()->SetTurnOnSafetyStop(21);
-
-  canopen_controller_->GetFrontDriver()->TurnOnSafetyStopChannel1();
-  canopen_controller_->GetRearDriver()->TurnOnSafetyStopChannel1();
+  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  roboteqs_mock_->GetDriver()->SetTurnOnSafetyStop(67);
+  roboteq_driver_->TurnOnSafetyStopChannel1();
 
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOnSafetyStop(), 1);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOnSafetyStop(), 1);
+  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetTurnOnSafetyStop(), 1);
 }
 
 TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel2)
 {
-  roboteqs_mock_->GetFrontDriver()->SetTurnOnSafetyStop(65);
-  roboteqs_mock_->GetRearDriver()->SetTurnOnSafetyStop(23);
-
-  canopen_controller_->GetFrontDriver()->TurnOnSafetyStopChannel2();
-  canopen_controller_->GetRearDriver()->TurnOnSafetyStopChannel2();
+  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  roboteqs_mock_->GetDriver()->SetTurnOnSafetyStop(65);
+  roboteq_driver_->TurnOnSafetyStopChannel2();
 
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOnSafetyStop(), 2);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOnSafetyStop(), 2);
+  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetTurnOnSafetyStop(), 2);
 }
 
 TEST_F(TestRoboteqDriver, WriteTimeout)
 {
-  roboteqs_mock_->GetFrontDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 200000);
+  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  roboteqs_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 200000);
   ASSERT_THROW(
-    canopen_controller_->GetFrontDriver()->TurnOnSafetyStopChannel1(), std::runtime_error);
+    roboteq_driver_->TurnOnSafetyStopChannel1(), std::runtime_error);
 }
 
 // OnCanError isn't tested, because it reacts to lower-level CAN errors (CRC), which are hard to
diff --git a/panther_hardware_interfaces/test/utils/fake_can_socket.hpp b/panther_hardware_interfaces/test/utils/fake_can_socket.hpp
new file mode 100644
index 00000000..e9ed9c82
--- /dev/null
+++ b/panther_hardware_interfaces/test/utils/fake_can_socket.hpp
@@ -0,0 +1,80 @@
+// 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_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_
+#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_
+
+#include <stdexcept>
+#include <string>
+
+#include <sys/socket.h>
+
+namespace panther_hardware_interfaces_test
+{
+
+class FakeCANSocket
+{
+public:
+  FakeCANSocket(const std::string & can_interface_name)
+  : can_interface_name_(can_interface_name), can_device_created_(false)
+  {
+  }
+
+  ~FakeCANSocket() { Deinitialize(); }
+
+  void Initialize()
+  {
+    if (system("sudo modprobe vcan") != 0) {
+      throw std::runtime_error("Failed to load vcan module");
+    }
+
+    // if link already exists, do not create it
+    const auto check_command = "ip link show " + can_interface_name_ + " > /dev/null 2>&1";
+    if (std::system(check_command.c_str()) != 0) {
+      const auto command = "sudo ip link add dev " + can_interface_name_ + " type vcan";
+      if (system(command.c_str()) != 0) {
+        throw std::runtime_error("Failed to add vcan device");
+      }
+    }
+
+    can_device_created_ = true;
+
+    const auto command = "sudo ip link set up " + can_interface_name_;
+    if (system(command.c_str()) != 0) {
+      throw std::runtime_error("Failed to set up vcan device");
+    }
+  }
+
+  void Deinitialize()
+  {
+    if (!can_device_created_) {
+      return;
+    }
+
+    std::string command = "sudo ip link delete " + can_interface_name_;
+    if (system(command.c_str()) != 0) {
+      throw std::runtime_error("Failed to delete vcan device");
+    }
+
+    can_device_created_ = false;
+  }
+
+private:
+  const std::string can_interface_name_;
+  bool can_device_created_;
+};
+
+}  // namespace panther_hardware_interfaces_test
+
+#endif  // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_
\ No newline at end of file
diff --git a/panther_hardware_interfaces/test/utils/roboteqs_mock.hpp b/panther_hardware_interfaces/test/utils/roboteqs_mock.hpp
index da6b4bb2..14fa4426 100644
--- a/panther_hardware_interfaces/test/utils/roboteqs_mock.hpp
+++ b/panther_hardware_interfaces/test/utils/roboteqs_mock.hpp
@@ -23,13 +23,13 @@
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
 
-#include <lely/coapp/slave.hpp>
-#include <lely/ev/loop.hpp>
-#include <lely/io2/linux/can.hpp>
-#include <lely/io2/posix/poll.hpp>
-#include <lely/io2/sys/io.hpp>
-#include <lely/io2/sys/sigset.hpp>
-#include <lely/io2/sys/timer.hpp>
+#include "lely/coapp/slave.hpp"
+#include "lely/ev/loop.hpp"
+#include "lely/io2/linux/can.hpp"
+#include "lely/io2/posix/poll.hpp"
+#include "lely/io2/sys/io.hpp"
+#include "lely/io2/sys/sigset.hpp"
+#include "lely/io2/sys/timer.hpp"
 
 namespace panther_hardware_interfaces_test
 {
@@ -307,21 +307,12 @@ class RoboteqsMock
         lely::io::CanChannel chan1(poll, exec);
         chan1.open(ctrl);
         lely::io::Timer timer1(poll, exec, CLOCK_MONOTONIC);
-        front_driver_ = std::make_shared<RoboteqSlave>(
+        driver_ = std::make_shared<RoboteqSlave>(
           timer1, chan1, slave_eds_path, slave1_eds_bin_path, 1);
 
-        lely::io::CanChannel chan2(poll, exec);
-        chan2.open(ctrl);
-        lely::io::Timer timer2(poll, exec, CLOCK_MONOTONIC);
-        rear_driver_ = std::make_shared<RoboteqSlave>(
-          timer2, chan2, slave_eds_path, slave2_eds_bin_path, 2);
-
-        front_driver_->Reset();
-        rear_driver_->Reset();
-        front_driver_->InitializeValues();
-        rear_driver_->InitializeValues();
-        front_driver_->StartPublishing(motors_states_period, driver_state_period);
-        rear_driver_->StartPublishing(motors_states_period, driver_state_period);
+        driver_->Reset();
+        driver_->InitializeValues();
+        driver_->StartPublishing(motors_states_period, driver_state_period);
 
         {
           std::lock_guard<std::mutex> lck_g(canopen_communication_started_mtx_);
@@ -331,8 +322,7 @@ class RoboteqsMock
 
         loop.run();
 
-        front_driver_->StopPublishing();
-        rear_driver_->StopPublishing();
+        driver_->StopPublishing();
       });
 
     if (!canopen_communication_started_.load()) {
@@ -355,14 +345,12 @@ class RoboteqsMock
       canopen_communication_thread_.join();
     }
 
-    front_driver_.reset();
-    rear_driver_.reset();
+    driver_.reset();
 
     canopen_communication_started_.store(false);
   }
 
-  std::shared_ptr<RoboteqSlave> GetFrontDriver() { return front_driver_; }
-  std::shared_ptr<RoboteqSlave> GetRearDriver() { return rear_driver_; }
+  std::shared_ptr<RoboteqSlave> GetDriver() { return driver_; }
 
 private:
   std::shared_ptr<lely::io::Context> ctx_;
@@ -373,8 +361,7 @@ class RoboteqsMock
   std::condition_variable canopen_communication_started_cond_;
   std::mutex canopen_communication_started_mtx_;
 
-  std::shared_ptr<RoboteqSlave> front_driver_;
-  std::shared_ptr<RoboteqSlave> rear_driver_;
+  std::shared_ptr<RoboteqSlave> driver_;
 };
 
 }  // namespace panther_hardware_interfaces_test

From cf7531fdb1b8d4dd89805197339c175d4424646e Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Wed, 21 Aug 2024 11:22:11 +0000
Subject: [PATCH 008/100] move test files

---
 panther_hardware_interfaces/CMakeLists.txt                    | 4 ++--
 .../motors_controller/test_canopen_controller.cpp             | 0
 .../panther_system/motors_controller/test_roboteq_driver.cpp  | 0
 3 files changed, 2 insertions(+), 2 deletions(-)
 rename panther_hardware_interfaces/test/{ => unit}/panther_system/motors_controller/test_canopen_controller.cpp (100%)
 rename panther_hardware_interfaces/test/{ => unit}/panther_system/motors_controller/test_roboteq_driver.cpp (100%)

diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt
index 8713f699..fa48f2a5 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/panther_hardware_interfaces/CMakeLists.txt
@@ -119,7 +119,7 @@ if(BUILD_TESTING)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_canopen_controller
-    test/panther_system/motors_controller/test_canopen_controller.cpp
+    test/unit/panther_system/motors_controller/test_canopen_controller.cpp
     src/panther_system/motors_controller/canopen_controller.cpp
     src/panther_system/motors_controller/roboteq_driver.cpp
     src/utils.cpp)
@@ -134,7 +134,7 @@ if(BUILD_TESTING)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_driver
-    test/panther_system/motors_controller/test_roboteq_driver.cpp
+    test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
     src/panther_system/motors_controller/canopen_controller.cpp
     src/panther_system/motors_controller/roboteq_driver.cpp
     src/utils.cpp)
diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_controller.cpp
similarity index 100%
rename from panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp
rename to panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_controller.cpp
diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
similarity index 100%
rename from panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_driver.cpp
rename to panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp

From 2683a9123f1f1774825163dcd6715d17c7c05c28 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Thu, 22 Aug 2024 09:07:16 +0000
Subject: [PATCH 009/100] update msgs names

---
 .docs/panther_ros2_api.drawio.svg             |   2 +-
 ROS_API.md                                    |   2 +-
 panther_battery/README.md                     |   2 +-
 .../battery/roboteq_battery.hpp               |  16 +--
 .../panther_battery/battery_driver_node.hpp   |   8 +-
 .../src/battery/roboteq_battery.cpp           |  26 ++---
 panther_battery/src/battery_driver_node.cpp   |   6 +-
 .../test/battery/test_roboteq_battery.cpp     |  53 ++++-----
 .../test_battery_driver_node_adc_single.cpp   |   2 +-
 .../test/test_battery_driver_node_roboteq.cpp |  16 +--
 .../test/utils/test_battery_driver_node.hpp   |  10 +-
 .../launch/controller.launch.py               |   2 +-
 panther_hardware_interfaces/README.md         |   2 +-
 .../panther_system_ros_interface.hpp          |  20 ++--
 .../src/panther_system/panther_system.cpp     |   2 +-
 .../panther_system_ros_interface.cpp          | 110 ++++++++----------
 .../panther_system/test_panther_system.cpp    |  48 ++++----
 .../test_panther_system_ros_interface.cpp     |  78 ++++++-------
 .../test/utils/test_constants.hpp             |   2 +-
 panther_manager/README.md                     |   2 +-
 .../panther_manager/safety_manager_node.hpp   |   8 +-
 panther_manager/src/safety_manager_node.cpp   |  11 +-
 .../test/test_safety_manager_node.cpp         |   8 +-
 23 files changed, 212 insertions(+), 224 deletions(-)

diff --git a/.docs/panther_ros2_api.drawio.svg b/.docs/panther_ros2_api.drawio.svg
index 29cefa8b..19f5bf61 100644
--- a/.docs/panther_ros2_api.drawio.svg
+++ b/.docs/panther_ros2_api.drawio.svg
@@ -1,4 +1,4 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!-- Do not edit this file with editors other than draw.io -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
-<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="1932px" height="1041px" viewBox="-0.5 -0.5 1932 1041" content="&lt;mxfile host=&quot;app.diagrams.net&quot; agent=&quot;Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/124.0.0.0 Safari/537.36&quot; scale=&quot;1&quot; border=&quot;0&quot; version=&quot;24.7.6&quot;&gt;&#10;  &lt;diagram id=&quot;5C2LWovSXbSQWriEvJtN&quot; name=&quot;Page-1&quot;&gt;&#10;    &lt;mxGraphModel dx=&quot;1350&quot; dy=&quot;1328&quot; grid=&quot;1&quot; gridSize=&quot;10&quot; guides=&quot;1&quot; tooltips=&quot;1&quot; connect=&quot;1&quot; arrows=&quot;1&quot; fold=&quot;1&quot; page=&quot;0&quot; pageScale=&quot;1&quot; pageWidth=&quot;850&quot; pageHeight=&quot;1100&quot; math=&quot;0&quot; shadow=&quot;0&quot;&gt;&#10;      &lt;root&gt;&#10;        &lt;mxCell id=&quot;0&quot; /&gt;&#10;        &lt;mxCell id=&quot;1&quot; parent=&quot;0&quot; /&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-23&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_localization&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;imageHeight=24;arcSize=6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;970&quot; y=&quot;165.15&quot; width=&quot;180&quot; height=&quot;100&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-179&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_controller&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=#FFFFFF;strokeWidth=6;strokeColor=#E6E6E6;imageHeight=24;arcSize=6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;840&quot; y=&quot;320&quot; width=&quot;600&quot; height=&quot;240&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-199&quot; value=&quot;&amp;lt;span style=&amp;quot;font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_lights&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=top;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2400&quot; y=&quot;380&quot; width=&quot;200&quot; height=&quot;450&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-202&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_hardware_interfaces&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=5;strokeColor=#E6E6E6;imageHeight=24;arcSize=6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;840&quot; y=&quot;590&quot; width=&quot;1150&quot; height=&quot;360.12&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-206&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_battery&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=top;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2027.36&quot; y=&quot;610&quot; width=&quot;256&quot; height=&quot;140&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-208&quot; value=&quot;&amp;lt;span style=&amp;quot;font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_manager&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1978.87&quot; y=&quot;270&quot; width=&quot;243&quot; height=&quot;277.55&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-211&quot; value=&quot;&amp;lt;span style=&amp;quot;font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_diagnostics&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=top;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2083.36&quot; y=&quot;110&quot; width=&quot;200&quot; height=&quot;120&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-24&quot; value=&quot;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot; style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;PantherSystem&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;align=center;labelBackgroundColor=none;fillColor=#CCCCCC;strokeColor=#E6E6E6;arcSize=6;verticalAlign=top;fontColor=#FFFFFF;gradientColor=none;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1155&quot; y=&quot;610.72&quot; width=&quot;815&quot; height=&quot;311.5&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; value=&quot;&amp;lt;span style=&amp;quot;color: rgb(255, 255, 255); text-align: left;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;hardware_controller&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ED3B42;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1660&quot; y=&quot;720.72&quot; width=&quot;285&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;GPIOController&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1416.88&quot; y=&quot;840.72&quot; width=&quot;160&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-28&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;MotorsController&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1345&quot; y=&quot;670.72&quot; width=&quot;190&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-29&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;CANopenController&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1176&quot; y=&quot;840.72&quot; width=&quot;200&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-30&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;RoboteqDriver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1201&quot; y=&quot;755.72&quot; width=&quot;150&quot; height=&quot;47.5&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-31&quot; value=&quot;CAN&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1207.25&quot; y=&quot;970.72&quot; width=&quot;137.5&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-32&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;labelBackgroundColor=none;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=16;fontColor=#888a85;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;fillColor=#FFFFFF;startArrow=block;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-29&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-31&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1204.25&quot; y=&quot;1210.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1291&quot; y=&quot;940.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-35&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=1;exitDx=0;exitDy=0;fillColor=#FFFFFF;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startArrow=block;endFill=1;&quot; parent=&quot;1&quot; source=&quot;NPlMt0osOJUE4kA7_BK4-15&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-79&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1501.13&quot; y=&quot;871.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1496.88&quot; y=&quot;970.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-38&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;EStopManager&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1415&quot; y=&quot;753.22&quot; width=&quot;160&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-56&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.488;entryY=-0.004;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.789;exitY=1;exitDx=0;exitDy=0;exitPerimeter=0;entryPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1496.78&quot; y=&quot;720.9200000000001&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1496.78&quot; y=&quot;753.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-58&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.789;exitY=1;exitDx=0;exitDy=0;exitPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1496.74&quot; y=&quot;803.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1496.74&quot; y=&quot;835.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-63&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.5;exitDx=0;exitDy=0;startFill=1;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-28&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-30&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1255&quot; y=&quot;688.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1255&quot; y=&quot;720.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-64&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;startFill=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-30&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-29&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1290&quot; y=&quot;810.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1291.34&quot; y=&quot;840.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-66&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.25;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;edgeStyle=orthogonalEdgeStyle;exitX=0;exitY=0.5;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-38&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1660&quot; y=&quot;766&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1593&quot; y=&quot;875.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-73&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.5;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.25;exitDx=0;exitDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-28&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1626&quot; y=&quot;695.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1545&quot; y=&quot;696.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-74&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.25;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.75;exitDx=0;exitDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1680&quot; y=&quot;797.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1599&quot; y=&quot;798.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;battery_driver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2057.36&quot; y=&quot;644.84&quot; width=&quot;190&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-77&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#888a85;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=0;exitDx=0;exitDy=0;fillColor=#FFFFFF;entryX=0.5;entryY=1;entryDx=0;entryDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-80&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2509.86&quot; y=&quot;1146.53&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2133.9400000000005&quot; y=&quot;720.7200000000001&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-79&quot; value=&quot;RPi GPIO&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1650&quot; y=&quot;970.72&quot; width=&quot;130&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-80&quot; value=&quot;ADC&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2083.61&quot; y=&quot;970.7199999999999&quot; width=&quot;137.5&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;imu_broadcaster&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;895&quot; y=&quot;474.72&quot; width=&quot;180&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;joint_state_broadcaster&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1190&quot; y=&quot;337.00000000000006&quot; width=&quot;240&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;drive_controller&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1070&quot; y=&quot;414.72&quot; width=&quot;180&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-95&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: left;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;controller_manager&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#FFFFFF;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;direction=north;horizontal=0;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;815&quot; y=&quot;330&quot; width=&quot;55&quot; height=&quot;220&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;daj83a5Ns2Ukew12XzJU-1&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;&quot; edge=&quot;1&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot;&gt;&#10;          &lt;mxGeometry relative=&quot;1&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;daj83a5Ns2Ukew12XzJU-2&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;&quot; edge=&quot;1&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot;&gt;&#10;          &lt;mxGeometry relative=&quot;1&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; value=&quot;&amp;lt;div style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot; style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;PantherImuSensor&amp;lt;/font&amp;gt;&amp;lt;/div&amp;gt;&amp;lt;div style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;align=center;labelBackgroundColor=none;fillColor=#CCCCCC;strokeColor=#E6E6E6;fontColor=#FFFFFF;arcSize=6;verticalAlign=top;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;860&quot; y=&quot;610.72&quot; width=&quot;250&quot; height=&quot;311.5&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-103&quot; value=&quot;IMU&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;916.25&quot; y=&quot;970.72&quot; width=&quot;137.5&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-106&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.5;exitY=1;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;660&quot; y=&quot;1060.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;721.25&quot; y=&quot;1060.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-116&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.25;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.687;exitY=0.972;exitDx=0;exitDy=0;exitPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-24&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1365&quot; y=&quot;380&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1365&quot; y=&quot;610.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-117&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.094;entryY=0.008;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.889;exitY=1.025;exitDx=0;exitDy=0;entryPerimeter=0;exitPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-24&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1235&quot; y=&quot;480.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1235&quot; y=&quot;610.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;ekf_filter&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;995&quot; y=&quot;175.72&quot; width=&quot;130&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-120&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;ImuFilter&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;880&quot; y=&quot;695.22&quot; width=&quot;210&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-121&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;Spatial&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;890&quot; y=&quot;821.34&quot; width=&quot;190&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-123&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=0;exitDx=0;exitDy=0;entryX=0.5;entryY=1;entryDx=0;entryDy=0;fillColor=default;endFill=1;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-121&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;984.5&quot; y=&quot;970.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;984.5&quot; y=&quot;922.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-124&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=0;exitDx=0;exitDy=0;entryX=0.5;entryY=1;entryDx=0;entryDy=0;fillColor=default;endFill=1;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-120&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;984.6600000000001&quot; y=&quot;821.34&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;984.6600000000001&quot; y=&quot;773.34&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-125&quot; value=&quot;&amp;lt;b&amp;gt;imu/data&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.25;exitY=0;exitDx=0;exitDy=0;align=right;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.8832&quot; y=&quot;15&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;940&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;975.47&quot; y=&quot;511.44000000000005&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-129&quot; value=&quot;&amp;lt;b&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;odometry/wheels&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;entryX=0.75;entryY=1;entryDx=0;entryDy=0;align=left;exitX=0.124;exitY=0.006;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.9376&quot; y=&quot;15&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1085&quot; y=&quot;305.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1085&quot; y=&quot;410.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-130&quot; value=&quot;&amp;lt;b&amp;gt;odometry/filtered&amp;lt;/b&amp;gt;&quot; style=&quot;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.25;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6758&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1028&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1040.3&quot; y=&quot;190.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-131&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.25;exitY=0;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.0769&quot; y=&quot;18&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;965&quot; y=&quot;40.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;965.47&quot; y=&quot;510.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-134&quot; value=&quot;&amp;lt;b&amp;gt;joint_states&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.588;exitY=0.013;exitDx=0;exitDy=0;align=right;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7994&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1330&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1330.0100000000002&quot; y=&quot;309.09000000000015&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-135&quot; value=&quot;&amp;lt;b&amp;gt;cmd_vel&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0.558;entryY=0.088;entryDx=0;entryDy=0;entryPerimeter=0;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.8578&quot; y=&quot;-15&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1170&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1170&quot; y=&quot;400&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-136&quot; value=&quot;&amp;lt;b&amp;gt;dynamic_joint_states&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.384;exitY=0.002;exitDx=0;exitDy=0;align=right;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.8069&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1281&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1280.0100000000002&quot; y=&quot;309.09000000000015&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-138&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;&amp;amp;nbsp;hardware/e_stop&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.876;exitY=-0.002;exitDx=0;exitDy=0;align=right;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; y=&quot;735.0912500000003&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-140&quot; value=&quot;&amp;lt;b&amp;gt;hardware/motor_controllers_state&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.63;exitY=0.019;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1840&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1830&quot; y=&quot;710&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-143&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;&amp;amp;nbsp;hardware/io_state&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.75;exitY=0;exitDx=0;exitDy=0;align=right;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1874&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1870&quot; y=&quot;735.09&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-144&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;hardware/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;e_stop_trigger&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.071;exitY=0.001;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1680&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1680.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-145&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;hardware/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;e_stop_reset&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.213;exitY=-0.004;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1720&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1720.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-148&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;hardware/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;&amp;lt;i&amp;gt;&amp;amp;lt;hw_component&amp;amp;gt;&amp;lt;/i&amp;gt;_enable&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.353;exitY=0.029;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1760&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1760.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-149&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;robot_state_publisher&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1365&quot; y=&quot;200&quot; width=&quot;240&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-150&quot; value=&quot;&amp;lt;b&amp;gt;robot_description&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.673;exitY=-0.008;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6903&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1530&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1530.01&quot; y=&quot;199.99999999999997&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-151&quot; value=&quot;&amp;lt;b&amp;gt;/tf&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6915&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1440&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1440&quot; y=&quot;200&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-152&quot; value=&quot;&amp;lt;b&amp;gt;/tf_static&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.5;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-149&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.68&quot; y=&quot;12&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1485&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1482.21&quot; y=&quot;220.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-153&quot; value=&quot;&amp;lt;b&amp;gt;/tf&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.5;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6747&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1060&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1070&quot; y=&quot;190.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-156&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;battery/battery_status&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=1;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9179&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2310&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2230&quot; y=&quot;670&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-158&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;jumpStyle=none;jumpSize=6;exitX=0.75;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2275&quot; y=&quot;530.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; y=&quot;730&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1874&quot; y=&quot;670&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-159&quot; value=&quot;&amp;lt;div style=&amp;quot;font-size: 20px;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;&amp;quot;&amp;gt;Bumper Lights&amp;lt;/span&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=16;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2420&quot; y=&quot;970.7200000000001&quot; width=&quot;160&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights_driver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2410&quot; y=&quot;721.47&quot; width=&quot;180&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights_controller&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2410&quot; y=&quot;420&quot; width=&quot;180&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-162&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#888a85;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#FFFFFF;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startArrow=none;startFill=0;exitX=0.5;exitY=1;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-159&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2243.75&quot; y=&quot;915.8200000000002&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2243.41&quot; y=&quot;966.3200000000002&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-165&quot; value=&quot;&amp;lt;b style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;lights/channel_1_frame&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=16;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=left;entryX=0.5;entryY=0;entryDx=0;entryDy=0;exitX=0.5;exitY=1;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7234&quot; y=&quot;-20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2560&quot; y=&quot;853.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2320&quot; y=&quot;853.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-167&quot; value=&quot;&amp;lt;b style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;lights/channel_2_frame&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=16;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=left;exitX=0.75;exitY=1;exitDx=0;exitDy=0;entryX=0.75;entryY=0;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7234&quot; y=&quot;-20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2560&quot; y=&quot;883.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2606.98&quot; y=&quot;655.8200000000002&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-168&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: center; font-size: 18px;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial; font-size: 18px;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights/&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial; text-align: right; font-size: 18px;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;set_animation&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=1;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=0;exitY=0.75;exitDx=0;exitDy=0;entryX=1;entryY=0.75;entryDx=0;entryDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; target=&quot;D_R0GpuGBRVuPwS73e86-2&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.8077&quot; y=&quot;12&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2204.63&quot; y=&quot;469.2799999999999&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2208.14&quot; y=&quot;837.225&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-170&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: center;&amp;quot;&amp;gt;&amp;lt;b&amp;gt;lights/set_brightness&amp;lt;/b&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=1;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9279&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2620&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2430.0050000000006&quot; y=&quot;815&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;safety_manager&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2012&quot; y=&quot;304.72&quot; width=&quot;190&quot; height=&quot;100&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;system_monitor&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2093.35&quot; y=&quot;140.57&quot; width=&quot;180&quot; height=&quot;70&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-183&quot; value=&quot;&amp;lt;b&amp;gt;system_status&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0;exitY=0.5;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7278&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2040&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1940&quot; y=&quot;120.00000000000004&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2040&quot; y=&quot;130&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2040&quot; y=&quot;40&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-184&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0.145;entryY=0.01;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0;exitY=0.5;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9108&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2040&quot; y=&quot;280.7221600000001&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2080&quot; y=&quot;130&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2040&quot; y=&quot;176&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-187&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.876;exitY=-0.012;exitDx=0;exitDy=0;align=right;exitPerimeter=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2000&quot; y=&quot;420&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; y=&quot;735.0912500000003&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1910&quot; y=&quot;355&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-189&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0;entryY=0.25;entryDx=0;entryDy=0;exitX=0.75;exitY=0;exitDx=0;exitDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2009&quot; y=&quot;399&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1870&quot; y=&quot;730&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-190&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0;entryY=0;entryDx=0;entryDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2000&quot; y=&quot;380&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1840&quot; y=&quot;720&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1840&quot; y=&quot;305&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-191&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.353;exitY=0.023;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;entryX=0;entryY=1;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9453&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2009&quot; y=&quot;359&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1760.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1761&quot; y=&quot;405&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-195&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=1;align=left;exitX=1;exitY=0.25;exitDx=0;exitDy=0;entryX=1;entryY=0.5;entryDx=0;entryDy=0;fontStyle=1&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.0992&quot; y=&quot;-22&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2229.24&quot; y=&quot;400&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2149.24&quot; y=&quot;658&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2266&quot; y=&quot;668&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;668&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;331&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-204&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.59;exitY=0.007;exitDx=0;exitDy=0;align=right;exitPerimeter=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;jumpSize=6;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-149&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.8002&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1330&quot; y=&quot;10&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1330.0100000000002&quot; y=&quot;309.09000000000015&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1332&quot; y=&quot;300&quot; /&gt;&#10;              &lt;mxPoint x=&quot;1332&quot; y=&quot;230&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-214&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#676965;startArrow=none;startFill=0;endArrow=none;horizontal=1;align=left;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;verticalAlign=middle;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;780.0300000000002&quot; y=&quot;220.26000000000002&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;10&quot; y=&quot;10&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;720&quot; y=&quot;219.68999999999997&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;780&quot; y=&quot;219.91&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-215&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#676965;startArrow=none;startFill=0;endArrow=none;horizontal=1;align=left;endFill=0;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1&quot; y=&quot;-10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;780&quot; y=&quot;179.75&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;10&quot; y=&quot;-10&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;720&quot; y=&quot;179.84000000000003&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;D_R0GpuGBRVuPwS73e86-2&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights_manager&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ED3B42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2012&quot; y=&quot;420&quot; width=&quot;190&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;D_R0GpuGBRVuPwS73e86-7&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=1;align=left;entryX=1;entryY=0.25;entryDx=0;entryDy=0;fontStyle=1;exitX=1;exitY=0.25;exitDx=0;exitDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; target=&quot;D_R0GpuGBRVuPwS73e86-2&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.0992&quot; y=&quot;-22&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2340&quot; y=&quot;389.24&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2300&quot; y=&quot;700&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;668&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;443&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;D_R0GpuGBRVuPwS73e86-9&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0;entryY=0.5;entryDx=0;entryDy=0;exitX=0.876;exitY=0.004;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;D_R0GpuGBRVuPwS73e86-2&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;3091&quot; y=&quot;289.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2910&quot; y=&quot;695.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-3&quot; value=&quot;&amp;lt;span style=&amp;quot;forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;forced-color-adjust: none;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;forced-color-adjust: none; font-size: 16px;&amp;quot;&amp;gt;hardware/&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;span style=&amp;quot;forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; font-size: 16px;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;forced-color-adjust: none;&amp;quot;&amp;gt;led_control_enable&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=1;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;verticalAlign=middle;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9141&quot; y=&quot;-14&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2214.63&quot; y=&quot;1267.0600000000002&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2210&quot; y=&quot;898.3400000000001&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-6&quot; value=&quot;&amp;lt;b&amp;gt;/tf&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.389;exitY=0.011;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.857&quot; y=&quot;17&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1140&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1144.23&quot; y=&quot;190.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-8&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;battery/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;charging_status&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=1;exitY=0.5;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9189&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2340&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2306&quot; y=&quot;678&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-14&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=0.071;exitY=0.012;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0;entryY=0.75;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1690&quot; y=&quot;10&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1620&quot; y=&quot;520&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1680&quot; y=&quot;380&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-15&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;GPIODriver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1650&quot; y=&quot;840.72&quot; width=&quot;130&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-17&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.75;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.75;exitDx=0;exitDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;NPlMt0osOJUE4kA7_BK4-15&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1670&quot; y=&quot;797&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1587&quot; y=&quot;863&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-2&quot; value=&quot;&amp;lt;b&amp;gt;diagnostics&amp;lt;/b&amp;gt;&quot; style=&quot;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.75;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6747&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1093&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1070&quot; y=&quot;186&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-3&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: left;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial;&amp;quot;&amp;gt;&amp;lt;b&amp;gt;diagnostics&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=1;exitY=0.75;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9189&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2370&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2257&quot; y=&quot;700&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-4&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;diagnostics&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6688&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2070&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2090&quot; y=&quot;186&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-6&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: left;&amp;quot;&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9189&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2370&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2257&quot; y=&quot;722&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-7&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;diagnostics&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.5;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1802&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1840&quot; y=&quot;731&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-8&quot; value=&quot;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ED3B42;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;720&quot; y=&quot;45&quot; width=&quot;60&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-11&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 20px; font-weight: 700; text-wrap: wrap;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot;&amp;gt;Legend:&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;text;html=1;align=center;verticalAlign=middle;resizable=0;points=[];autosize=1;strokeColor=none;fillColor=none;fontSize=16;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;700&quot; y=&quot;-4.85&quot; width=&quot;100&quot; height=&quot;40&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-12&quot; value=&quot;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=#E6E6E6;fillColor=#CCCCCC;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;720&quot; y=&quot;85&quot; width=&quot;60&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-14&quot; value=&quot;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=#B0B3AC;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;720&quot; y=&quot;125&quot; width=&quot;60&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-18&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Nodes&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;45&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-19&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Plugin&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;85&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-20&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Objects&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;125&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-21&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Topics&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;165&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-22&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Services&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;205&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;Y5d-tTOP6FVwuIk47fc7-3&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=none;startWidth=5.555555555555555;endWidth=5.555555555555555;startFill=0;endFill=1;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;720&quot; y=&quot;260&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;780&quot; y=&quot;259.57&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;Y5d-tTOP6FVwuIk47fc7-4&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Data Transfer&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;245&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;      &lt;/root&gt;&#10;    &lt;/mxGraphModel&gt;&#10;  &lt;/diagram&gt;&#10;&lt;/mxfile&gt;&#10;" resource="https://app.diagrams.net/#G1LlhIvfnfv5NcG4CBD7yvBf7X81yUhw3e"><defs/><g><g data-cell-id="0"><g data-cell-id="1"><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-23"><g><rect x="270" y="175.15" width="180" height="100" rx="6" ry="6" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 272px; margin-left: 271px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">panther_localization</font></span></div></div></div></foreignObject><text x="360" y="272" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_localization</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-179"><g><rect x="140" y="330" width="600" height="240" rx="14.4" ry="14.4" fill-opacity="0.9" fill="#ffffff" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 598px; height: 1px; padding-top: 567px; margin-left: 141px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">panther_controller</font></span></div></div></div></foreignObject><text x="440" y="567" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-199"><g><rect x="1700" y="390" width="200" height="450" rx="30" ry="30" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 397px; margin-left: 1701px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;"><font color="#1a1a1a">panther_lights</font></span></div></div></div></foreignObject><text x="1800" y="409" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_lights</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-202"><g><rect x="140" y="600" width="1150" height="360.12" rx="21.61" ry="21.61" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="5" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 1148px; height: 1px; padding-top: 957px; margin-left: 141px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">panther_hardware_interfaces</font></span></div></div></div></foreignObject><text x="715" y="957" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_hardware_interfaces</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-206"><g><rect x="1327.36" y="620" width="256" height="140" rx="21" ry="21" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 254px; height: 1px; padding-top: 627px; margin-left: 1328px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">panther_battery</font></span></div></div></div></foreignObject><text x="1455" y="639" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_battery</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-208"><g><rect x="1278.87" y="280" width="243" height="277.55" rx="36.45" ry="36.45" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 241px; height: 1px; padding-top: 555px; margin-left: 1280px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;"><font color="#1a1a1a">panther_manager</font></span></div></div></div></foreignObject><text x="1400" y="555" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_manager</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-211"><g><rect x="1383.36" y="120" width="200" height="120" rx="18" ry="18" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 127px; margin-left: 1384px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;"><font color="#1a1a1a">panther_diagnostics</font></span></div></div></div></foreignObject><text x="1483" y="139" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_diagnostics</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-24"><g><rect x="455" y="620.72" width="815" height="311.5" rx="18.69" ry="18.69" fill="#cccccc" stroke="#e6e6e6" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 813px; height: 1px; padding-top: 632px; margin-left: 456px;"><div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 24px;" color="#1a1a1a">PantherSystem</font></div></div></div></foreignObject><text x="863" y="651" fill="#FFFFFF" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">PantherSystem</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-26"><g><rect x="960" y="730.72" width="285" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 283px; height: 1px; padding-top: 778px; margin-left: 961px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><span style="color: rgb(255, 255, 255); text-align: left;"><font style="font-size: 18px;">hardware_controller</font></span></div></div></div></foreignObject><text x="1103" y="783" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">hardware_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-27"><g><rect x="716.88" y="850.72" width="160" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 878px; margin-left: 718px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">GPIOController</font></div></div></div></foreignObject><text x="797" y="883" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">GPIOController</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-28"><g><rect x="645" y="680.72" width="190" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 708px; margin-left: 646px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">MotorsController</font></div></div></div></foreignObject><text x="740" y="713" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">MotorsController</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-29"><g><rect x="476" y="850.72" width="200" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 878px; margin-left: 477px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">CANopenController</font></div></div></div></foreignObject><text x="576" y="883" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">CANopenController</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-30"><g><rect x="501" y="765.72" width="150" height="47.5" rx="7.13" ry="7.13" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 148px; height: 1px; padding-top: 791px; margin-left: 502px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">RoboteqDriver</font></div></div></div></foreignObject><text x="576" y="797" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">RoboteqDriver</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-31"><g><rect x="507.25" y="980.72" width="137.5" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 136px; height: 1px; padding-top: 1011px; margin-left: 508px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">CAN</div></div></div></foreignObject><text x="576" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">CAN</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-32"><g><path d="M 576 919.43 L 576 962.01" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 576 907.43 L 582 919.43 L 570 919.43 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 576 974.01 L 570 962.01 L 582 962.01 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-35"><g><path d="M 1015 919.43 L 1015 962.01" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1015 907.43 L 1021 919.43 L 1009 919.43 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 1015 974.01 L 1009 962.01 L 1021 962.01 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-38"><g><rect x="715" y="763.22" width="160" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 790px; margin-left: 716px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">EStopManager</font></div></div></div></foreignObject><text x="795" y="796" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">EStopManager</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-56"><g><path d="M 796.78 743.27 L 796.78 750.87" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 796.78 734.27 L 801.28 743.27 L 792.28 743.27 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 796.78 759.87 L 792.28 750.87 L 801.28 750.87 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-58"><g><path d="M 796.79 825.57 L 796.83 838.37" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 796.75 816.57 L 801.29 825.56 L 792.29 825.59 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 796.87 847.37 L 792.33 838.38 L 801.33 838.35 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-63"><g><path d="M 632.65 705.72 L 586 705.7 Q 576 705.7 576 715.7 L 576 753.37" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 641.65 705.72 L 632.64 710.22 L 632.65 701.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 576 762.37 L 571.5 753.37 L 580.5 753.37 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-64"><g><path d="M 576 825.57 L 576 838.37" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 576 816.57 L 580.5 825.57 L 571.5 825.57 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 576 847.37 L 571.5 838.37 L 580.5 838.37 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-66"><g><path d="M 947.65 775.72 L 887.35 775.72" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 956.65 775.72 L 947.65 780.22 L 947.65 771.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 878.35 775.72 L 887.35 771.22 L 887.35 780.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-73"><g><path d="M 947.65 753.22 L 907.5 753.2 Q 897.5 753.2 897.5 743.2 L 897.5 715.7 Q 897.5 705.7 887.5 705.7 L 847.35 705.72" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 956.65 753.22 L 947.64 757.72 L 947.65 748.72 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 838.35 705.72 L 847.35 701.22 L 847.36 710.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-74"><g><path d="M 947.65 798.21 L 928.5 798.2 Q 918.5 798.2 918.5 808.2 L 918.5 853.2 Q 918.5 863.2 908.5 863.2 L 889.23 863.21" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 956.65 798.22 L 947.64 802.71 L 947.65 793.71 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 880.23 863.22 L 889.23 858.71 L 889.24 867.71 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-75"><g><rect x="1357.36" y="654.84" width="190" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 702px; margin-left: 1358px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">battery_driver</font></div></div></div></foreignObject><text x="1452" y="708" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">battery_driver</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-77"><g><path d="M 1452.36 980.72 L 1452.36 763.55" fill="none" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1452.36 751.55 L 1458.36 763.55 L 1446.36 763.55 Z" fill="#888a85" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-79"><g><rect x="950" y="980.72" width="130" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 1011px; margin-left: 951px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">RPi GPIO</div></div></div></foreignObject><text x="1015" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">RPi GPIO</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-80"><g><rect x="1383.61" y="980.72" width="137.5" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 136px; height: 1px; padding-top: 1011px; margin-left: 1385px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">ADC</div></div></div></foreignObject><text x="1452" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">ADC</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-84"><g><rect x="195" y="484.72" width="180" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 517px; margin-left: 196px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">imu_broadcaster</font></div></div></div></foreignObject><text x="285" y="522" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">imu_broadcaster</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-85"><g><rect x="490" y="347" width="240" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 379px; margin-left: 491px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">joint_state_broadcaster</font></div></div></div></foreignObject><text x="610" y="385" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">joint_state_broadcaster</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-86"><g><rect x="370" y="424.72" width="180" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 457px; margin-left: 371px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">drive_controller</font></div></div></div></foreignObject><text x="460" y="462" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">drive_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-95"><g><rect x="32.5" y="422.5" width="220" height="55" rx="8.25" ry="8.25" fill="#ed3b42" stroke="none" transform="rotate(270,142.5,450)" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 144.5 450)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 218px; height: 1px; padding-top: 450px; margin-left: 36px;"><div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><div style="text-align: left;"><span style="background-color: initial;"><font style="font-size: 18px;">controller_manager</font></span></div></div></div></div></foreignObject><text x="145" y="456" fill="#FFFFFF" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">controller_manager</text></switch></g></g></g><g data-cell-id="daj83a5Ns2Ukew12XzJU-1"><g><path d="M 285 620.72 L 285 551.09" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 285 545.84 L 288.5 552.84 L 285 551.09 L 281.5 552.84 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="daj83a5Ns2Ukew12XzJU-2"><g><path d="M 285 620.72 L 285 551.09" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 285 545.84 L 288.5 552.84 L 285 551.09 L 281.5 552.84 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-99"><g><rect x="160" y="620.72" width="250" height="311.5" rx="15" ry="15" fill="#cccccc" stroke="#e6e6e6" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 248px; height: 1px; padding-top: 632px; margin-left: 161px;"><div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><div style="font-size: 24px;"><font style="font-size: 24px;" color="#1a1a1a">PantherImuSensor</font></div><div style="font-size: 24px;"><br /></div></div></div></div></foreignObject><text x="285" y="651" fill="#FFFFFF" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">PantherImuSensor&#xa;</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-103"><g><rect x="216.25" y="980.72" width="137.5" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 136px; height: 1px; padding-top: 1011px; margin-left: 217px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">IMU</div></div></div></foreignObject><text x="285" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">IMU</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-106"><g><path d="M 285 563.43 L 285 602.01" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 285 551.43 L 291 563.43 L 279 563.43 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 285 614.01 L 279 602.01 L 291 602.01 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-116"><g><path d="M 655.22 424.02 L 658.41 602.02" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 655 412.03 L 661.22 423.92 L 649.22 424.13 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 658.63 614.01 L 652.41 602.12 L 664.41 601.91 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-117"><g><path d="M 530.24 504.93 L 531.39 604.51" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 530.1 492.93 L 536.24 504.86 L 524.24 505 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 531.53 616.5 L 525.39 604.57 L 537.39 604.44 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-118"><g><rect x="295" y="185.72" width="130" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 218px; margin-left: 296px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">ekf_filter</font></div></div></div></foreignObject><text x="360" y="223" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">ekf_filter</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-120"><g><rect x="180" y="705.22" width="210" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 208px; height: 1px; padding-top: 732px; margin-left: 181px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">ImuFilter</font></div></div></div></foreignObject><text x="285" y="738" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">ImuFilter</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-121"><g><rect x="190" y="831.34" width="190" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 858px; margin-left: 191px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">Spatial</font></div></div></div></foreignObject><text x="285" y="864" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">Spatial</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-123"><g><path d="M 284.5 980.22 L 284.91 900.05" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 284.97 888.05 L 290.91 900.08 L 278.91 900.02 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-124"><g><path d="M 284.66 831.34 L 284.92 773.93" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 284.97 761.93 L 290.92 773.95 L 278.92 773.9 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-125"><g><path d="M 240 484.72 L 240 257.9 Q 240 247.9 240 237.9 L 240 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 240 14.47 L 245 24.47 L 240 21.97 L 235 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 223.05934473723292 38.25198888361385)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 38px; margin-left: 223px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>imu/data</b></div></div></div></foreignObject><text x="223" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">imu/data</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-129"><g><path d="M 392.32 425.08 L 392.49 257.69" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 392.5 250.19 L 397.49 260.2 L 392.49 257.69 L 387.49 260.19 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 379.32602896003704 418.0649495053864)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 1px; height: 1px; padding-top: 418px; margin-left: 379px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b><font style="font-size: 18px;">odometry/wheels</font></b></div></div></div></foreignObject><text x="379" y="423" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px">odometry/wheels</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-130"><g><path d="M 327.5 185.72 L 327.97 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 327.99 14.47 L 332.96 24.49 L 327.97 21.97 L 322.96 24.46 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 315.918317869348 38.69214084552837)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 316px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>odometry/filtered</b></div></div></div></foreignObject><text x="316" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">odometry/filtered</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-131"><g><path d="M 240 484.72 L 240 225.7 Q 240 215.7 250 215.7 L 283.03 215.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 290.53 215.72 L 280.53 220.71 L 283.03 215.72 L 280.53 210.71 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-134"><g><path d="M 631.12 347.78 L 631.1 189 Q 631.1 179 631.03 169 L 630.08 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 630.03 14.47 L 635.09 24.44 L 630.08 21.97 L 625.09 24.5 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 618.1017614886418 44.4101760279836)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 44px; margin-left: 618px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>joint_states</b></div></div></div></foreignObject><text x="618" y="50" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">joint_states</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-135"><g><path d="M 470 10 L 470.48 207.9 Q 470.5 217.9 470.5 227.9 L 470.44 418.03" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 470.44 425.53 L 465.44 415.53 L 470.44 418.03 L 475.44 415.53 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 453.0721932435777 40.03598817157367)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 453px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>cmd_vel</b></div></div></div></foreignObject><text x="453" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">cmd_vel</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-136"><g><path d="M 582.16 347.12 L 582.2 189 Q 582.2 179 582.13 169 L 581.09 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 581.03 14.47 L 586.1 24.44 L 581.09 21.97 L 576.1 24.51 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 569.0971461347985 42.85018642894648)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 43px; margin-left: 569px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>dynamic_joint_states</b></div></div></div></foreignObject><text x="569" y="48" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">dynamic_joint_states</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-138"><g><path d="M 1209.66 730.54 L 1209.7 380.9 Q 1209.7 370.9 1209.71 360.9 L 1209.99 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1210 14.47 L 1214.99 24.48 L 1209.99 21.97 L 1204.99 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1198.040658419977 39.35417466263085)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 1198px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;"> hardware/e_stop</b></div></div></div></foreignObject><text x="1198" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end"> hardware/e_stop</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-140"><g><path d="M 1139.55 732.43 L 1139.6 380.9 Q 1139.6 370.9 1139.61 360.9 L 1139.99 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1140 14.47 L 1144.98 24.48 L 1139.99 21.97 L 1134.98 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1128.0420299914413 40.34417494560757)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1128px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>hardware/motor_controllers_state</b></div></div></div></foreignObject><text x="1128" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/motor_controllers_state</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-143"><g><path d="M 1173.75 730.72 L 1173.8 380.9 Q 1173.8 370.9 1173.81 360.9 L 1173.99 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1174 14.47 L 1178.99 24.47 L 1173.99 21.97 L 1168.99 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1162.0407692556278 39.434175835366155)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 1162px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;"> hardware/io_state</b></div></div></div></foreignObject><text x="1162" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end"> hardware/io_state</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-144"><g><path d="M 980.23 721.07 L 980.2 380.9 Q 980.2 370.9 980.19 360.9 L 980 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 980.23 728.57 L 975.23 718.57 L 980.23 721.07 L 985.23 718.57 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 967.0417251957298 40.1241734010298)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 967px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;">hardware/</b><b>e_stop_trigger</b></div></div></div></foreignObject><text x="967" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/e_stop_trigger</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-145"><g><path d="M 1020.7 720.62 L 1020.7 380.9 Q 1020.7 370.9 1020.68 360.9 L 1020 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1020.7 728.12 L 1015.7 718.12 L 1020.7 720.62 L 1025.7 718.12 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1007.0409632102225 39.57417226182657)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1007px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;">hardware/</b><b>e_stop_reset</b></div></div></div></foreignObject><text x="1007" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/e_stop_reset</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-148"><g><path d="M 1060.6 723.59 L 1060.6 380.9 Q 1060.6 370.9 1060.58 360.9 L 1060 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1060.6 731.09 L 1055.6 721.09 L 1060.6 723.59 L 1065.6 721.09 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1047.0407831051025 39.444172386302576)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 1047px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;">hardware/</b><b><i>&lt;hw_component&gt;</i>_enable</b></div></div></div></foreignObject><text x="1047" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/&lt;hw_component&gt;_enable</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-149"><g><rect x="665" y="210" width="240" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 242px; margin-left: 666px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">robot_state_publisher</font></div></div></div></foreignObject><text x="785" y="248" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">robot_state_publisher</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-150"><g><path d="M 830.01 210 L 830 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 830 14.47 L 835 24.47 L 830 21.97 L 825 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 818.0015500125105 41.00050021124936)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 41px; margin-left: 818px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>robot_description</b></div></div></div></foreignObject><text x="818" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">robot_description</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-151"><g><path d="M 740 210 L 740 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 740 14.47 L 745 24.47 L 740 21.97 L 735 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 728 41)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 41px; margin-left: 728px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf</b></div></div></div></foreignObject><text x="728" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-152"><g><path d="M 785 210 L 785 120.5 Q 785 110.5 785 100.5 L 785 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 785 14.47 L 790 24.47 L 785 21.97 L 780 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 771.1618442375116 41.560542301394726)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 42px; margin-left: 771px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf_static</b></div></div></div></foreignObject><text x="771" y="47" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf_static</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-153"><g><path d="M 360 185.72 L 360 108.4 Q 360 98.4 360 88.4 L 360 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 360 14.47 L 365 24.47 L 360 21.97 L 355 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 348.1597807831874 38.27752278433415)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 38px; margin-left: 348px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf</b></div></div></div></foreignObject><text x="348" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-156"><g><path d="M 1547.36 677.34 L 1600.5 677.31 Q 1610.5 677.3 1610.49 667.3 L 1610.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1610 14.47 L 1615.01 24.47 L 1610.01 21.97 L 1605.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1598.0228112826962 40.44768432687242)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1598px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">battery/battery_status</b></div></div></div></foreignObject><text x="1598" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">battery/battery_status</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-158"><g><path d="M 1173.75 730.72 L 1173.79 690 Q 1173.8 680 1183.8 680 L 1345.39 680" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1352.89 680 L 1342.89 685 L 1345.39 680 L 1342.89 675 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-159"><g><rect x="1720" y="980.72" width="160" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 1011px; margin-left: 1721px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><div style="font-size: 20px;"><span style="">Bumper Lights</span><br /></div></div></div></div></foreignObject><text x="1800" y="1016" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="16px" text-anchor="middle" font-weight="bold">Bumper Lights&#xa;</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-160"><g><rect x="1710" y="731.47" width="180" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 778px; margin-left: 1711px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">lights_driver</font></div></div></div></foreignObject><text x="1800" y="784" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">lights_driver</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-161"><g><rect x="1710" y="430" width="180" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 477px; margin-left: 1711px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">lights_controller</font></div></div></div></foreignObject><text x="1800" y="483" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">lights_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-162"><g><path d="M 1800 821.47 L 1800 962.01" fill="none" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1800 974.01 L 1794 962.01 L 1806 962.01 Z" fill="#888a85" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-165"><g><path d="M 1800 520 L 1800 719.5" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1800 727 L 1795 717 L 1800 719.5 L 1805 717 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1782 701)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 1px; height: 1px; padding-top: 701px; margin-left: 1782px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="font-size: 16px;"><font style="font-size: 16px;">lights/channel_1_frame</font></b></div></div></div></foreignObject><text x="1782" y="706" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="16px">lights/channel_1_frame</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-167"><g><path d="M 1845 520 L 1845 719.5" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1845 727 L 1840 717 L 1845 719.5 L 1850 717 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1827 701)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 1px; height: 1px; padding-top: 701px; margin-left: 1827px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="font-size: 16px;"><font style="font-size: 16px;">lights/channel_2_frame</font></b></div></div></div></foreignObject><text x="1827" y="706" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="16px">lights/channel_2_frame</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-168"><g><path d="M 1700.26 497.5 L 1502 497.5" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1707.76 497.5 L 1697.76 502.5 L 1700.26 497.5 L 1697.76 492.5 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 510px; margin-left: 1687px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: center; font-size: 18px;"><span style="background-color: initial; font-size: 18px;"><b style="font-size: 18px;"><font style="font-size: 18px;">lights/</font></b></span><span style="background-color: initial; text-align: right; font-size: 18px;"><b style="font-size: 18px;">set_animation</b></span></div></div></div></div></foreignObject><text x="1687" y="515" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">lights/set_animation</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-170"><g><path d="M 1899.74 753.98 L 1910.5 753.99 Q 1920.5 754 1920.49 744 L 1920 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1892.24 753.97 L 1902.24 748.98 L 1899.74 753.98 L 1902.23 758.98 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1907.0184835596563 37.50689698330439)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 38px; margin-left: 1907px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: center;"><b>lights/set_brightness</b><br /></div></div></div></div></foreignObject><text x="1907" y="43" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">lights/set_brightness&#xa;</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-173"><g><rect x="1312" y="314.72" width="190" height="100" rx="15" ry="15" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 367px; margin-left: 1313px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">safety_manager</font></div></div></div></foreignObject><text x="1407" y="372" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">safety_manager</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-174"><g><rect x="1393.35" y="150.57" width="180" height="70" rx="10.5" ry="10.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 188px; margin-left: 1394px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">system_monitor</font></div></div></div></foreignObject><text x="1483" y="193" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">system_monitor</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-183"><g><path d="M 1393.35 185.57 L 1350 185.59 Q 1340 185.6 1340 175.6 L 1340 150 Q 1340 140 1340 130 L 1340 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1340 14.47 L 1345 24.47 L 1340 21.97 L 1335 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1328 40.95000843486335)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 41px; margin-left: 1328px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>system_status</b></div></div></div></foreignObject><text x="1328" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">system_status</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-184"><g><path d="M 1393.35 185.57 L 1349.6 185.59 Q 1339.6 185.6 1339.6 195.6 L 1339.55 303.75" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1339.55 311.25 L 1334.56 301.25 L 1339.55 303.75 L 1344.56 301.25 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-187"><g><path d="M 1209.66 729.64 L 1209.7 374.7 Q 1209.7 364.7 1219.7 364.7 L 1300.03 364.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 364.72 L 1297.53 369.72 L 1300.03 364.72 L 1297.53 359.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-189"><g><path d="M 1173.75 730.72 L 1173.8 349.7 Q 1173.8 339.7 1183.8 339.7 L 1300.03 339.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 339.72 L 1297.53 344.72 L 1300.03 339.72 L 1297.53 334.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-190"><g><path d="M 1140 730 L 1140 324.7 Q 1140 314.7 1150 314.7 L 1300.03 314.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 314.72 L 1297.53 319.72 L 1300.03 314.72 L 1297.53 309.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-191"><g><path d="M 1060.6 723.05 L 1060.6 424.7 Q 1060.6 414.7 1070.6 414.7 L 1312 414.72" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1060.6 730.55 L 1055.6 720.55 L 1060.6 723.05 L 1065.6 720.55 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-195"><g><path d="M 1547.36 677.34 L 1556.68 677.32 Q 1566 677.3 1576 677.46 L 1600 677.84 Q 1610 678 1610 668 L 1610 374.7 Q 1610 364.7 1600 364.7 L 1513.97 364.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1506.47 364.72 L 1516.47 359.72 L 1513.97 364.72 L 1516.47 369.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-204"><g><path d="M 631.6 347.42 L 631.6 320 Q 631.6 310 631.66 300 L 631.94 250 Q 632 240 642 240 L 653.03 240" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 660.53 240 L 650.53 245 L 653.03 240 L 650.53 235 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-214"><g><path d="M 20 230.3 L 80.03 230.26" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-215"><g><path d="M 20 189.84 L 80 189.75" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/></g></g><g data-cell-id="D_R0GpuGBRVuPwS73e86-2"><g><rect x="1312" y="430" width="190" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 477px; margin-left: 1313px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">lights_manager</font></div></div></div></foreignObject><text x="1407" y="483" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">lights_manager</text></switch></g></g></g><g data-cell-id="D_R0GpuGBRVuPwS73e86-7"><g><path d="M 1547.36 677.34 L 1600 677.31 Q 1610 677.3 1610 667.3 L 1610 462.5 Q 1610 452.5 1600 452.5 L 1513.97 452.5" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1506.47 452.5 L 1516.47 447.5 L 1513.97 452.5 L 1516.47 457.5 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="D_R0GpuGBRVuPwS73e86-9"><g><path d="M 1209.66 731.08 L 1209.7 485 Q 1209.7 475 1219.7 475 L 1300.03 475" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 475 L 1297.53 480 L 1300.03 475 L 1297.53 470 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-3"><g><path d="M 1254.74 775.72 L 1467.5 775.7 Q 1477.5 775.7 1487.5 775.73 L 1710 776.47" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1247.24 775.72 L 1257.24 770.72 L 1254.74 775.72 L 1257.24 780.72 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 790px; margin-left: 1687px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><span style="forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><b style="forced-color-adjust: none;"><font style="forced-color-adjust: none; font-size: 16px;">hardware/</font></b></span><span style="forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; font-size: 16px;"><b style="forced-color-adjust: none;">led_control_enable</b></span></div></div></div></foreignObject><text x="1687" y="796" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/led_control_enable</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-6"><g><path d="M 440.02 425.38 L 440 227.9 Q 440 217.9 440 207.9 L 440 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 440 14.47 L 445 24.47 L 440 21.97 L 435 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 421.0719118764564 39.92140072145239)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 421px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf</b></div></div></div></foreignObject><text x="421" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-8"><g><path d="M 1547.36 699.84 L 1630.5 699.8 Q 1640.5 699.8 1640.49 689.8 L 1640.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1640 14.47 L 1645.01 24.47 L 1640.01 21.97 L 1635.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1628.0231543966713 41.947429886303745)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 42px; margin-left: 1628px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">battery/</b><b>charging_status</b></div></div></div></foreignObject><text x="1628" y="47" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">battery/charging_status</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-14"><g><path d="M 980.23 722.06 L 980.2 399.7 Q 980.2 389.7 990.2 389.7 L 1312 389.72" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 980.23 729.56 L 975.23 719.56 L 980.23 722.06 L 985.23 719.56 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-15"><g><rect x="950" y="850.72" width="130" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 878px; margin-left: 951px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">GPIODriver</font></div></div></div></foreignObject><text x="1015" y="883" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">GPIODriver</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-17"><g><path d="M 937.65 888.22 L 889.23 888.22" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 946.65 888.22 L 937.65 892.72 L 937.65 883.72 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 880.23 888.22 L 889.23 883.72 L 889.23 892.72 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-2"><g><path d="M 392.5 185.72 L 392.97 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 392.99 14.47 L 397.96 24.49 L 392.97 21.97 L 387.96 24.46 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 380.918317869348 38.69214084552837)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 381px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>diagnostics</b></div></div></div></foreignObject><text x="381" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-3"><g><path d="M 1547.36 722.34 L 1660.5 722.3 Q 1670.5 722.3 1670.49 712.3 L 1670.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1670 14.47 L 1675.01 24.47 L 1670.01 21.97 L 1665.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1658.02347584129 43.447193258440564)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 43px; margin-left: 1658px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: left;"><span style="background-color: initial;"><b>diagnostics</b></span></div></div></div></div></foreignObject><text x="1658" y="49" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-4"><g><path d="M 1393.35 168.07 L 1380.5 168.09 Q 1370.5 168.1 1370.47 158.1 L 1370.04 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1370.01 14.47 L 1375.05 24.46 L 1370.04 21.97 L 1365.05 24.49 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1358.0947706300105 39.98228594656824)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1358px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">diagnostics</b></div></div></div></foreignObject><text x="1358" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-6"><g><path d="M 1710 753.97 L 1680.5 753.99 Q 1670.5 754 1670.49 744 L 1670.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1670 14.47 L 1675.01 24.47 L 1670.01 21.97 L 1665.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1658.021171728833 41.506892718330505)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 42px; margin-left: 1658px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: left;"><br /></div></div></div></div></foreignObject><text x="1658" y="47" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">&#xa;</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-7"><g><path d="M 1102.5 730.72 L 1102.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1102 14.47 L 1107.01 24.47 L 1102.01 21.97 L 1097.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1090.0206207924284 39.727103790911734)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1090px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">diagnostics</b></div></div></div></foreignObject><text x="1090" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-8"><g><rect x="20" y="55" width="60" height="30" rx="4.5" ry="4.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-11"><g><rect x="0" y="5.15" width="100" height="40" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 25px; margin-left: 50px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: nowrap;"><span style="font-size: 20px; font-weight: 700; text-wrap: wrap;"><font color="#888a85">Legend:</font></span></div></div></div></foreignObject><text x="50" y="30" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="16px" text-anchor="middle">Legend:</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-12"><g><rect x="20" y="95" width="60" height="30" rx="4.5" ry="4.5" fill="#cccccc" stroke="#e6e6e6" stroke-width="6" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-14"><g><rect x="20" y="135" width="60" height="30" rx="4.5" ry="4.5" fill="#ffffff" stroke="#b0b3ac" stroke-width="2" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-18"><g><rect x="95" y="55" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 62px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Nodes</font></b></div></div></div></foreignObject><text x="97" y="80" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Nodes</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-19"><g><rect x="95" y="95" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 102px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Plugin</font></b></div></div></div></foreignObject><text x="97" y="120" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Plugin</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-20"><g><rect x="95" y="135" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 142px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Objects</font></b></div></div></div></foreignObject><text x="97" y="160" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Objects</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-21"><g><rect x="95" y="175" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 182px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Topics</font></b></div></div></div></foreignObject><text x="97" y="200" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Topics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-22"><g><rect x="95" y="215" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 222px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Services</font></b></div></div></div></foreignObject><text x="97" y="240" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Services</text></switch></g></g></g><g data-cell-id="Y5d-tTOP6FVwuIk47fc7-3"><g><path d="M 20 270 L 61.29 269.7" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 73.29 269.62 L 61.34 275.7 L 61.25 263.7 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="Y5d-tTOP6FVwuIk47fc7-4"><g><rect x="95" y="255" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 262px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Data Transfer</font></b></div></div></div></foreignObject><text x="97" y="280" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Data Trans...</text></switch></g></g></g></g></g></g><switch><g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/><a transform="translate(0,-5)" xlink:href="https://www.drawio.com/doc/faq/svg-export-text-problems" target="_blank"><text text-anchor="middle" font-size="10px" x="50%" y="100%">Text is not SVG - cannot display</text></a></switch></svg>
+<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="1932px" height="1041px" viewBox="-0.5 -0.5 1932 1041" content="&lt;mxfile host=&quot;app.diagrams.net&quot; agent=&quot;Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/124.0.0.0 Safari/537.36&quot; scale=&quot;1&quot; border=&quot;0&quot; version=&quot;24.7.6&quot;&gt;&#10;  &lt;diagram id=&quot;5C2LWovSXbSQWriEvJtN&quot; name=&quot;Page-1&quot;&gt;&#10;    &lt;mxGraphModel dx=&quot;1350&quot; dy=&quot;1328&quot; grid=&quot;1&quot; gridSize=&quot;10&quot; guides=&quot;1&quot; tooltips=&quot;1&quot; connect=&quot;1&quot; arrows=&quot;1&quot; fold=&quot;1&quot; page=&quot;0&quot; pageScale=&quot;1&quot; pageWidth=&quot;850&quot; pageHeight=&quot;1100&quot; math=&quot;0&quot; shadow=&quot;0&quot;&gt;&#10;      &lt;root&gt;&#10;        &lt;mxCell id=&quot;0&quot; /&gt;&#10;        &lt;mxCell id=&quot;1&quot; parent=&quot;0&quot; /&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-23&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_localization&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;imageHeight=24;arcSize=6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;970&quot; y=&quot;165.15&quot; width=&quot;180&quot; height=&quot;100&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-179&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_controller&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=#FFFFFF;strokeWidth=6;strokeColor=#E6E6E6;imageHeight=24;arcSize=6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;840&quot; y=&quot;320&quot; width=&quot;600&quot; height=&quot;240&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-199&quot; value=&quot;&amp;lt;span style=&amp;quot;font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_lights&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=top;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2400&quot; y=&quot;380&quot; width=&quot;200&quot; height=&quot;450&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-202&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_hardware_interfaces&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=5;strokeColor=#E6E6E6;imageHeight=24;arcSize=6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;840&quot; y=&quot;590&quot; width=&quot;1150&quot; height=&quot;360.12&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-206&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_battery&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=top;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2027.36&quot; y=&quot;610&quot; width=&quot;256&quot; height=&quot;140&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-208&quot; value=&quot;&amp;lt;span style=&amp;quot;font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_manager&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1978.87&quot; y=&quot;270&quot; width=&quot;243&quot; height=&quot;277.55&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-211&quot; value=&quot;&amp;lt;span style=&amp;quot;font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_diagnostics&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=top;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2083.36&quot; y=&quot;110&quot; width=&quot;200&quot; height=&quot;120&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-24&quot; value=&quot;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot; style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;PantherSystem&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;align=center;labelBackgroundColor=none;fillColor=#CCCCCC;strokeColor=#E6E6E6;arcSize=6;verticalAlign=top;fontColor=#FFFFFF;gradientColor=none;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1155&quot; y=&quot;610.72&quot; width=&quot;815&quot; height=&quot;311.5&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; value=&quot;&amp;lt;span style=&amp;quot;color: rgb(255, 255, 255); text-align: left;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;hardware_controller&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ED3B42;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1660&quot; y=&quot;720.72&quot; width=&quot;285&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;GPIOController&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1416.88&quot; y=&quot;840.72&quot; width=&quot;160&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-28&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;MotorsController&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1345&quot; y=&quot;670.72&quot; width=&quot;190&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-29&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;CANopenController&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1176&quot; y=&quot;840.72&quot; width=&quot;200&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-30&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;RoboteqDriver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1201&quot; y=&quot;755.72&quot; width=&quot;150&quot; height=&quot;47.5&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-31&quot; value=&quot;CAN&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1207.25&quot; y=&quot;970.72&quot; width=&quot;137.5&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-32&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;labelBackgroundColor=none;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=16;fontColor=#888a85;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;fillColor=#FFFFFF;startArrow=block;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-29&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-31&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1204.25&quot; y=&quot;1210.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1291&quot; y=&quot;940.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-35&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=1;exitDx=0;exitDy=0;fillColor=#FFFFFF;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startArrow=block;endFill=1;&quot; parent=&quot;1&quot; source=&quot;NPlMt0osOJUE4kA7_BK4-15&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-79&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1501.13&quot; y=&quot;871.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1496.88&quot; y=&quot;970.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-38&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;EStopManager&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1415&quot; y=&quot;753.22&quot; width=&quot;160&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-56&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.488;entryY=-0.004;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.789;exitY=1;exitDx=0;exitDy=0;exitPerimeter=0;entryPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1496.78&quot; y=&quot;720.9200000000001&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1496.78&quot; y=&quot;753.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-58&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.789;exitY=1;exitDx=0;exitDy=0;exitPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1496.74&quot; y=&quot;803.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1496.74&quot; y=&quot;835.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-63&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.5;exitDx=0;exitDy=0;startFill=1;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-28&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-30&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1255&quot; y=&quot;688.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1255&quot; y=&quot;720.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-64&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;startFill=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-30&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-29&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1290&quot; y=&quot;810.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1291.34&quot; y=&quot;840.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-66&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.25;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;edgeStyle=orthogonalEdgeStyle;exitX=0;exitY=0.5;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-38&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1660&quot; y=&quot;766&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1593&quot; y=&quot;875.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-73&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.5;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.25;exitDx=0;exitDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-28&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1626&quot; y=&quot;695.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1545&quot; y=&quot;696.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-74&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.25;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.75;exitDx=0;exitDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1680&quot; y=&quot;797.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1599&quot; y=&quot;798.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;battery_driver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2057.36&quot; y=&quot;644.84&quot; width=&quot;190&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-77&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#888a85;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=0;exitDx=0;exitDy=0;fillColor=#FFFFFF;entryX=0.5;entryY=1;entryDx=0;entryDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-80&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2509.86&quot; y=&quot;1146.53&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2133.9400000000005&quot; y=&quot;720.7200000000001&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-79&quot; value=&quot;RPi GPIO&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1650&quot; y=&quot;970.72&quot; width=&quot;130&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-80&quot; value=&quot;ADC&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2083.61&quot; y=&quot;970.7199999999999&quot; width=&quot;137.5&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;imu_broadcaster&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;895&quot; y=&quot;474.72&quot; width=&quot;180&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;joint_state_broadcaster&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1190&quot; y=&quot;337.00000000000006&quot; width=&quot;240&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;drive_controller&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1070&quot; y=&quot;414.72&quot; width=&quot;180&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-95&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: left;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;controller_manager&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#FFFFFF;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;direction=north;horizontal=0;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;815&quot; y=&quot;330&quot; width=&quot;55&quot; height=&quot;220&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;daj83a5Ns2Ukew12XzJU-1&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;&quot; edge=&quot;1&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot;&gt;&#10;          &lt;mxGeometry relative=&quot;1&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;daj83a5Ns2Ukew12XzJU-2&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;&quot; edge=&quot;1&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot;&gt;&#10;          &lt;mxGeometry relative=&quot;1&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; value=&quot;&amp;lt;div style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot; style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;PantherImuSensor&amp;lt;/font&amp;gt;&amp;lt;/div&amp;gt;&amp;lt;div style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;align=center;labelBackgroundColor=none;fillColor=#CCCCCC;strokeColor=#E6E6E6;fontColor=#FFFFFF;arcSize=6;verticalAlign=top;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;860&quot; y=&quot;610.72&quot; width=&quot;250&quot; height=&quot;311.5&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-103&quot; value=&quot;IMU&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;916.25&quot; y=&quot;970.72&quot; width=&quot;137.5&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-106&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.5;exitY=1;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;660&quot; y=&quot;1060.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;721.25&quot; y=&quot;1060.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-116&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.25;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.687;exitY=0.972;exitDx=0;exitDy=0;exitPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-24&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1365&quot; y=&quot;380&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1365&quot; y=&quot;610.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-117&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.094;entryY=0.008;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.889;exitY=1.025;exitDx=0;exitDy=0;entryPerimeter=0;exitPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-24&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1235&quot; y=&quot;480.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1235&quot; y=&quot;610.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;ekf_filter&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;995&quot; y=&quot;175.72&quot; width=&quot;130&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-120&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;ImuFilter&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;880&quot; y=&quot;695.22&quot; width=&quot;210&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-121&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;Spatial&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;890&quot; y=&quot;821.34&quot; width=&quot;190&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-123&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=0;exitDx=0;exitDy=0;entryX=0.5;entryY=1;entryDx=0;entryDy=0;fillColor=default;endFill=1;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-121&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;984.5&quot; y=&quot;970.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;984.5&quot; y=&quot;922.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-124&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=0;exitDx=0;exitDy=0;entryX=0.5;entryY=1;entryDx=0;entryDy=0;fillColor=default;endFill=1;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-120&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;984.6600000000001&quot; y=&quot;821.34&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;984.6600000000001&quot; y=&quot;773.34&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-125&quot; value=&quot;&amp;lt;b&amp;gt;imu/data&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.25;exitY=0;exitDx=0;exitDy=0;align=right;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.8832&quot; y=&quot;15&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;940&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;975.47&quot; y=&quot;511.44000000000005&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-129&quot; value=&quot;&amp;lt;b&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;odometry/wheels&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;entryX=0.75;entryY=1;entryDx=0;entryDy=0;align=left;exitX=0.124;exitY=0.006;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.9376&quot; y=&quot;15&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1085&quot; y=&quot;305.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1085&quot; y=&quot;410.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-130&quot; value=&quot;&amp;lt;b&amp;gt;odometry/filtered&amp;lt;/b&amp;gt;&quot; style=&quot;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.25;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6758&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1028&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1040.3&quot; y=&quot;190.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-131&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.25;exitY=0;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.0769&quot; y=&quot;18&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;965&quot; y=&quot;40.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;965.47&quot; y=&quot;510.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-134&quot; value=&quot;&amp;lt;b&amp;gt;joint_states&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.588;exitY=0.013;exitDx=0;exitDy=0;align=right;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7994&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1330&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1330.0100000000002&quot; y=&quot;309.09000000000015&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-135&quot; value=&quot;&amp;lt;b&amp;gt;cmd_vel&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0.558;entryY=0.088;entryDx=0;entryDy=0;entryPerimeter=0;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.8578&quot; y=&quot;-15&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1170&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1170&quot; y=&quot;400&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-136&quot; value=&quot;&amp;lt;b&amp;gt;dynamic_joint_states&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.384;exitY=0.002;exitDx=0;exitDy=0;align=right;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.8069&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1281&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1280.0100000000002&quot; y=&quot;309.09000000000015&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-138&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;&amp;amp;nbsp;hardware/e_stop&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.876;exitY=-0.002;exitDx=0;exitDy=0;align=right;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; y=&quot;735.0912500000003&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-140&quot; value=&quot;&amp;lt;b&amp;gt;hardware/robot_driver_state&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.63;exitY=0.019;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1840&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1830&quot; y=&quot;710&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-143&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;&amp;amp;nbsp;hardware/io_state&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.75;exitY=0;exitDx=0;exitDy=0;align=right;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1874&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1870&quot; y=&quot;735.09&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-144&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;hardware/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;e_stop_trigger&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.071;exitY=0.001;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1680&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1680.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-145&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;hardware/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;e_stop_reset&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.213;exitY=-0.004;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1720&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1720.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-148&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;hardware/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;&amp;lt;i&amp;gt;&amp;amp;lt;hw_component&amp;amp;gt;&amp;lt;/i&amp;gt;_enable&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.353;exitY=0.029;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1760&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1760.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-149&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;robot_state_publisher&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1365&quot; y=&quot;200&quot; width=&quot;240&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-150&quot; value=&quot;&amp;lt;b&amp;gt;robot_description&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.673;exitY=-0.008;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6903&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1530&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1530.01&quot; y=&quot;199.99999999999997&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-151&quot; value=&quot;&amp;lt;b&amp;gt;/tf&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6915&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1440&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1440&quot; y=&quot;200&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-152&quot; value=&quot;&amp;lt;b&amp;gt;/tf_static&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.5;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-149&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.68&quot; y=&quot;12&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1485&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1482.21&quot; y=&quot;220.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-153&quot; value=&quot;&amp;lt;b&amp;gt;/tf&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.5;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6747&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1060&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1070&quot; y=&quot;190.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-156&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;battery/battery_status&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=1;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9179&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2310&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2230&quot; y=&quot;670&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-158&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;jumpStyle=none;jumpSize=6;exitX=0.75;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2275&quot; y=&quot;530.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; y=&quot;730&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1874&quot; y=&quot;670&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-159&quot; value=&quot;&amp;lt;div style=&amp;quot;font-size: 20px;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;&amp;quot;&amp;gt;Bumper Lights&amp;lt;/span&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=16;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2420&quot; y=&quot;970.7200000000001&quot; width=&quot;160&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights_driver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2410&quot; y=&quot;721.47&quot; width=&quot;180&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights_controller&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2410&quot; y=&quot;420&quot; width=&quot;180&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-162&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#888a85;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#FFFFFF;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startArrow=none;startFill=0;exitX=0.5;exitY=1;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-159&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2243.75&quot; y=&quot;915.8200000000002&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2243.41&quot; y=&quot;966.3200000000002&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-165&quot; value=&quot;&amp;lt;b style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;lights/channel_1_frame&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=16;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=left;entryX=0.5;entryY=0;entryDx=0;entryDy=0;exitX=0.5;exitY=1;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7234&quot; y=&quot;-20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2560&quot; y=&quot;853.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2320&quot; y=&quot;853.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-167&quot; value=&quot;&amp;lt;b style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;lights/channel_2_frame&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=16;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=left;exitX=0.75;exitY=1;exitDx=0;exitDy=0;entryX=0.75;entryY=0;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7234&quot; y=&quot;-20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2560&quot; y=&quot;883.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2606.98&quot; y=&quot;655.8200000000002&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-168&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: center; font-size: 18px;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial; font-size: 18px;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights/&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial; text-align: right; font-size: 18px;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;set_animation&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=1;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=0;exitY=0.75;exitDx=0;exitDy=0;entryX=1;entryY=0.75;entryDx=0;entryDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; target=&quot;D_R0GpuGBRVuPwS73e86-2&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.8077&quot; y=&quot;12&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2204.63&quot; y=&quot;469.2799999999999&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2208.14&quot; y=&quot;837.225&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-170&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: center;&amp;quot;&amp;gt;&amp;lt;b&amp;gt;lights/set_brightness&amp;lt;/b&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=1;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9279&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2620&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2430.0050000000006&quot; y=&quot;815&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;safety_manager&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2012&quot; y=&quot;304.72&quot; width=&quot;190&quot; height=&quot;100&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;system_monitor&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2093.35&quot; y=&quot;140.57&quot; width=&quot;180&quot; height=&quot;70&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-183&quot; value=&quot;&amp;lt;b&amp;gt;system_status&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0;exitY=0.5;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7278&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2040&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1940&quot; y=&quot;120.00000000000004&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2040&quot; y=&quot;130&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2040&quot; y=&quot;40&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-184&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0.145;entryY=0.01;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0;exitY=0.5;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9108&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2040&quot; y=&quot;280.7221600000001&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2080&quot; y=&quot;130&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2040&quot; y=&quot;176&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-187&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.876;exitY=-0.012;exitDx=0;exitDy=0;align=right;exitPerimeter=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2000&quot; y=&quot;420&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; y=&quot;735.0912500000003&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1910&quot; y=&quot;355&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-189&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0;entryY=0.25;entryDx=0;entryDy=0;exitX=0.75;exitY=0;exitDx=0;exitDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2009&quot; y=&quot;399&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1870&quot; y=&quot;730&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-190&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0;entryY=0;entryDx=0;entryDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2000&quot; y=&quot;380&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1840&quot; y=&quot;720&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1840&quot; y=&quot;305&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-191&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.353;exitY=0.023;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;entryX=0;entryY=1;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9453&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2009&quot; y=&quot;359&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1760.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1761&quot; y=&quot;405&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-195&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=1;align=left;exitX=1;exitY=0.25;exitDx=0;exitDy=0;entryX=1;entryY=0.5;entryDx=0;entryDy=0;fontStyle=1&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.0992&quot; y=&quot;-22&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2229.24&quot; y=&quot;400&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2149.24&quot; y=&quot;658&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2266&quot; y=&quot;668&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;668&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;331&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-204&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.59;exitY=0.007;exitDx=0;exitDy=0;align=right;exitPerimeter=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;jumpSize=6;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-149&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.8002&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1330&quot; y=&quot;10&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1330.0100000000002&quot; y=&quot;309.09000000000015&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1332&quot; y=&quot;300&quot; /&gt;&#10;              &lt;mxPoint x=&quot;1332&quot; y=&quot;230&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-214&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#676965;startArrow=none;startFill=0;endArrow=none;horizontal=1;align=left;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;verticalAlign=middle;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;780.0300000000002&quot; y=&quot;220.26000000000002&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;10&quot; y=&quot;10&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;720&quot; y=&quot;219.68999999999997&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;780&quot; y=&quot;219.91&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-215&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#676965;startArrow=none;startFill=0;endArrow=none;horizontal=1;align=left;endFill=0;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1&quot; y=&quot;-10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;780&quot; y=&quot;179.75&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;10&quot; y=&quot;-10&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;720&quot; y=&quot;179.84000000000003&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;D_R0GpuGBRVuPwS73e86-2&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights_manager&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ED3B42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2012&quot; y=&quot;420&quot; width=&quot;190&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;D_R0GpuGBRVuPwS73e86-7&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=1;align=left;entryX=1;entryY=0.25;entryDx=0;entryDy=0;fontStyle=1;exitX=1;exitY=0.25;exitDx=0;exitDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; target=&quot;D_R0GpuGBRVuPwS73e86-2&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.0992&quot; y=&quot;-22&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2340&quot; y=&quot;389.24&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2300&quot; y=&quot;700&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;668&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;443&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;D_R0GpuGBRVuPwS73e86-9&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0;entryY=0.5;entryDx=0;entryDy=0;exitX=0.876;exitY=0.004;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;D_R0GpuGBRVuPwS73e86-2&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;3091&quot; y=&quot;289.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2910&quot; y=&quot;695.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-3&quot; value=&quot;&amp;lt;span style=&amp;quot;forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;forced-color-adjust: none;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;forced-color-adjust: none; font-size: 16px;&amp;quot;&amp;gt;hardware/&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;span style=&amp;quot;forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; font-size: 16px;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;forced-color-adjust: none;&amp;quot;&amp;gt;led_control_enable&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=1;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;verticalAlign=middle;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9141&quot; y=&quot;-14&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2214.63&quot; y=&quot;1267.0600000000002&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2210&quot; y=&quot;898.3400000000001&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-6&quot; value=&quot;&amp;lt;b&amp;gt;/tf&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.389;exitY=0.011;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.857&quot; y=&quot;17&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1140&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1144.23&quot; y=&quot;190.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-8&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;battery/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;charging_status&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=1;exitY=0.5;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9189&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2340&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2306&quot; y=&quot;678&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-14&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=0.071;exitY=0.012;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0;entryY=0.75;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1690&quot; y=&quot;10&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1620&quot; y=&quot;520&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1680&quot; y=&quot;380&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-15&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;GPIODriver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1650&quot; y=&quot;840.72&quot; width=&quot;130&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-17&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.75;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.75;exitDx=0;exitDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;NPlMt0osOJUE4kA7_BK4-15&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1670&quot; y=&quot;797&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1587&quot; y=&quot;863&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-2&quot; value=&quot;&amp;lt;b&amp;gt;diagnostics&amp;lt;/b&amp;gt;&quot; style=&quot;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.75;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6747&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1093&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1070&quot; y=&quot;186&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-3&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: left;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial;&amp;quot;&amp;gt;&amp;lt;b&amp;gt;diagnostics&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=1;exitY=0.75;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9189&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2370&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2257&quot; y=&quot;700&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-4&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;diagnostics&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6688&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2070&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2090&quot; y=&quot;186&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-6&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: left;&amp;quot;&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9189&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2370&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2257&quot; y=&quot;722&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-7&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;diagnostics&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.5;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1802&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1840&quot; y=&quot;731&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-8&quot; value=&quot;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ED3B42;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;720&quot; y=&quot;45&quot; width=&quot;60&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-11&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 20px; font-weight: 700; text-wrap: wrap;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot;&amp;gt;Legend:&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;text;html=1;align=center;verticalAlign=middle;resizable=0;points=[];autosize=1;strokeColor=none;fillColor=none;fontSize=16;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;700&quot; y=&quot;-4.85&quot; width=&quot;100&quot; height=&quot;40&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-12&quot; value=&quot;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=#E6E6E6;fillColor=#CCCCCC;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;720&quot; y=&quot;85&quot; width=&quot;60&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-14&quot; value=&quot;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=#B0B3AC;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;720&quot; y=&quot;125&quot; width=&quot;60&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-18&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Nodes&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;45&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-19&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Plugin&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;85&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-20&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Objects&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;125&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-21&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Topics&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;165&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-22&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Services&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;205&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;Y5d-tTOP6FVwuIk47fc7-3&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=none;startWidth=5.555555555555555;endWidth=5.555555555555555;startFill=0;endFill=1;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;720&quot; y=&quot;260&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;780&quot; y=&quot;259.57&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;Y5d-tTOP6FVwuIk47fc7-4&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Data Transfer&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;245&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;      &lt;/root&gt;&#10;    &lt;/mxGraphModel&gt;&#10;  &lt;/diagram&gt;&#10;&lt;/mxfile&gt;&#10;" resource="https://app.diagrams.net/#G1LlhIvfnfv5NcG4CBD7yvBf7X81yUhw3e"><defs/><g><g data-cell-id="0"><g data-cell-id="1"><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-23"><g><rect x="270" y="175.15" width="180" height="100" rx="6" ry="6" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 272px; margin-left: 271px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">panther_localization</font></span></div></div></div></foreignObject><text x="360" y="272" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_localization</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-179"><g><rect x="140" y="330" width="600" height="240" rx="14.4" ry="14.4" fill-opacity="0.9" fill="#ffffff" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 598px; height: 1px; padding-top: 567px; margin-left: 141px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">panther_controller</font></span></div></div></div></foreignObject><text x="440" y="567" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-199"><g><rect x="1700" y="390" width="200" height="450" rx="30" ry="30" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 397px; margin-left: 1701px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;"><font color="#1a1a1a">panther_lights</font></span></div></div></div></foreignObject><text x="1800" y="409" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_lights</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-202"><g><rect x="140" y="600" width="1150" height="360.12" rx="21.61" ry="21.61" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="5" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 1148px; height: 1px; padding-top: 957px; margin-left: 141px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">panther_hardware_interfaces</font></span></div></div></div></foreignObject><text x="715" y="957" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_hardware_interfaces</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-206"><g><rect x="1327.36" y="620" width="256" height="140" rx="21" ry="21" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 254px; height: 1px; padding-top: 627px; margin-left: 1328px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">panther_battery</font></span></div></div></div></foreignObject><text x="1455" y="639" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_battery</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-208"><g><rect x="1278.87" y="280" width="243" height="277.55" rx="36.45" ry="36.45" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 241px; height: 1px; padding-top: 555px; margin-left: 1280px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;"><font color="#1a1a1a">panther_manager</font></span></div></div></div></foreignObject><text x="1400" y="555" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_manager</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-211"><g><rect x="1383.36" y="120" width="200" height="120" rx="18" ry="18" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 127px; margin-left: 1384px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;"><font color="#1a1a1a">panther_diagnostics</font></span></div></div></div></foreignObject><text x="1483" y="139" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_diagnostics</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-24"><g><rect x="455" y="620.72" width="815" height="311.5" rx="18.69" ry="18.69" fill="#cccccc" stroke="#e6e6e6" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 813px; height: 1px; padding-top: 632px; margin-left: 456px;"><div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 24px;" color="#1a1a1a">PantherSystem</font></div></div></div></foreignObject><text x="863" y="651" fill="#FFFFFF" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">PantherSystem</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-26"><g><rect x="960" y="730.72" width="285" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 283px; height: 1px; padding-top: 778px; margin-left: 961px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><span style="color: rgb(255, 255, 255); text-align: left;"><font style="font-size: 18px;">hardware_controller</font></span></div></div></div></foreignObject><text x="1103" y="783" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">hardware_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-27"><g><rect x="716.88" y="850.72" width="160" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 878px; margin-left: 718px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">GPIOController</font></div></div></div></foreignObject><text x="797" y="883" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">GPIOController</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-28"><g><rect x="645" y="680.72" width="190" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 708px; margin-left: 646px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">MotorsController</font></div></div></div></foreignObject><text x="740" y="713" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">MotorsController</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-29"><g><rect x="476" y="850.72" width="200" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 878px; margin-left: 477px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">CANopenController</font></div></div></div></foreignObject><text x="576" y="883" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">CANopenController</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-30"><g><rect x="501" y="765.72" width="150" height="47.5" rx="7.13" ry="7.13" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 148px; height: 1px; padding-top: 791px; margin-left: 502px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">RoboteqDriver</font></div></div></div></foreignObject><text x="576" y="797" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">RoboteqDriver</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-31"><g><rect x="507.25" y="980.72" width="137.5" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 136px; height: 1px; padding-top: 1011px; margin-left: 508px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">CAN</div></div></div></foreignObject><text x="576" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">CAN</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-32"><g><path d="M 576 919.43 L 576 962.01" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 576 907.43 L 582 919.43 L 570 919.43 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 576 974.01 L 570 962.01 L 582 962.01 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-35"><g><path d="M 1015 919.43 L 1015 962.01" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1015 907.43 L 1021 919.43 L 1009 919.43 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 1015 974.01 L 1009 962.01 L 1021 962.01 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-38"><g><rect x="715" y="763.22" width="160" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 790px; margin-left: 716px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">EStopManager</font></div></div></div></foreignObject><text x="795" y="796" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">EStopManager</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-56"><g><path d="M 796.78 743.27 L 796.78 750.87" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 796.78 734.27 L 801.28 743.27 L 792.28 743.27 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 796.78 759.87 L 792.28 750.87 L 801.28 750.87 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-58"><g><path d="M 796.79 825.57 L 796.83 838.37" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 796.75 816.57 L 801.29 825.56 L 792.29 825.59 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 796.87 847.37 L 792.33 838.38 L 801.33 838.35 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-63"><g><path d="M 632.65 705.72 L 586 705.7 Q 576 705.7 576 715.7 L 576 753.37" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 641.65 705.72 L 632.64 710.22 L 632.65 701.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 576 762.37 L 571.5 753.37 L 580.5 753.37 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-64"><g><path d="M 576 825.57 L 576 838.37" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 576 816.57 L 580.5 825.57 L 571.5 825.57 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 576 847.37 L 571.5 838.37 L 580.5 838.37 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-66"><g><path d="M 947.65 775.72 L 887.35 775.72" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 956.65 775.72 L 947.65 780.22 L 947.65 771.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 878.35 775.72 L 887.35 771.22 L 887.35 780.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-73"><g><path d="M 947.65 753.22 L 907.5 753.2 Q 897.5 753.2 897.5 743.2 L 897.5 715.7 Q 897.5 705.7 887.5 705.7 L 847.35 705.72" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 956.65 753.22 L 947.64 757.72 L 947.65 748.72 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 838.35 705.72 L 847.35 701.22 L 847.36 710.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-74"><g><path d="M 947.65 798.21 L 928.5 798.2 Q 918.5 798.2 918.5 808.2 L 918.5 853.2 Q 918.5 863.2 908.5 863.2 L 889.23 863.21" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 956.65 798.22 L 947.64 802.71 L 947.65 793.71 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 880.23 863.22 L 889.23 858.71 L 889.24 867.71 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-75"><g><rect x="1357.36" y="654.84" width="190" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 702px; margin-left: 1358px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">battery_driver</font></div></div></div></foreignObject><text x="1452" y="708" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">battery_driver</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-77"><g><path d="M 1452.36 980.72 L 1452.36 763.55" fill="none" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1452.36 751.55 L 1458.36 763.55 L 1446.36 763.55 Z" fill="#888a85" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-79"><g><rect x="950" y="980.72" width="130" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 1011px; margin-left: 951px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">RPi GPIO</div></div></div></foreignObject><text x="1015" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">RPi GPIO</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-80"><g><rect x="1383.61" y="980.72" width="137.5" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 136px; height: 1px; padding-top: 1011px; margin-left: 1385px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">ADC</div></div></div></foreignObject><text x="1452" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">ADC</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-84"><g><rect x="195" y="484.72" width="180" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 517px; margin-left: 196px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">imu_broadcaster</font></div></div></div></foreignObject><text x="285" y="522" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">imu_broadcaster</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-85"><g><rect x="490" y="347" width="240" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 379px; margin-left: 491px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">joint_state_broadcaster</font></div></div></div></foreignObject><text x="610" y="385" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">joint_state_broadcaster</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-86"><g><rect x="370" y="424.72" width="180" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 457px; margin-left: 371px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">drive_controller</font></div></div></div></foreignObject><text x="460" y="462" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">drive_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-95"><g><rect x="32.5" y="422.5" width="220" height="55" rx="8.25" ry="8.25" fill="#ed3b42" stroke="none" transform="rotate(270,142.5,450)" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 144.5 450)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 218px; height: 1px; padding-top: 450px; margin-left: 36px;"><div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><div style="text-align: left;"><span style="background-color: initial;"><font style="font-size: 18px;">controller_manager</font></span></div></div></div></div></foreignObject><text x="145" y="456" fill="#FFFFFF" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">controller_manager</text></switch></g></g></g><g data-cell-id="daj83a5Ns2Ukew12XzJU-1"><g><path d="M 285 620.72 L 285 551.09" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 285 545.84 L 288.5 552.84 L 285 551.09 L 281.5 552.84 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="daj83a5Ns2Ukew12XzJU-2"><g><path d="M 285 620.72 L 285 551.09" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 285 545.84 L 288.5 552.84 L 285 551.09 L 281.5 552.84 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-99"><g><rect x="160" y="620.72" width="250" height="311.5" rx="15" ry="15" fill="#cccccc" stroke="#e6e6e6" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 248px; height: 1px; padding-top: 632px; margin-left: 161px;"><div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><div style="font-size: 24px;"><font style="font-size: 24px;" color="#1a1a1a">PantherImuSensor</font></div><div style="font-size: 24px;"><br /></div></div></div></div></foreignObject><text x="285" y="651" fill="#FFFFFF" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">PantherImuSensor&#xa;</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-103"><g><rect x="216.25" y="980.72" width="137.5" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 136px; height: 1px; padding-top: 1011px; margin-left: 217px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">IMU</div></div></div></foreignObject><text x="285" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">IMU</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-106"><g><path d="M 285 563.43 L 285 602.01" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 285 551.43 L 291 563.43 L 279 563.43 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 285 614.01 L 279 602.01 L 291 602.01 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-116"><g><path d="M 655.22 424.02 L 658.41 602.02" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 655 412.03 L 661.22 423.92 L 649.22 424.13 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 658.63 614.01 L 652.41 602.12 L 664.41 601.91 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-117"><g><path d="M 530.24 504.93 L 531.39 604.51" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 530.1 492.93 L 536.24 504.86 L 524.24 505 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 531.53 616.5 L 525.39 604.57 L 537.39 604.44 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-118"><g><rect x="295" y="185.72" width="130" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 218px; margin-left: 296px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">ekf_filter</font></div></div></div></foreignObject><text x="360" y="223" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">ekf_filter</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-120"><g><rect x="180" y="705.22" width="210" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 208px; height: 1px; padding-top: 732px; margin-left: 181px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">ImuFilter</font></div></div></div></foreignObject><text x="285" y="738" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">ImuFilter</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-121"><g><rect x="190" y="831.34" width="190" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 858px; margin-left: 191px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">Spatial</font></div></div></div></foreignObject><text x="285" y="864" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">Spatial</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-123"><g><path d="M 284.5 980.22 L 284.91 900.05" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 284.97 888.05 L 290.91 900.08 L 278.91 900.02 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-124"><g><path d="M 284.66 831.34 L 284.92 773.93" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 284.97 761.93 L 290.92 773.95 L 278.92 773.9 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-125"><g><path d="M 240 484.72 L 240 257.9 Q 240 247.9 240 237.9 L 240 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 240 14.47 L 245 24.47 L 240 21.97 L 235 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 223.05934473723292 38.25198888361385)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 38px; margin-left: 223px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>imu/data</b></div></div></div></foreignObject><text x="223" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">imu/data</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-129"><g><path d="M 392.32 425.08 L 392.49 257.69" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 392.5 250.19 L 397.49 260.2 L 392.49 257.69 L 387.49 260.19 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 379.32602896003704 418.0649495053864)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 1px; height: 1px; padding-top: 418px; margin-left: 379px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b><font style="font-size: 18px;">odometry/wheels</font></b></div></div></div></foreignObject><text x="379" y="423" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px">odometry/wheels</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-130"><g><path d="M 327.5 185.72 L 327.97 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 327.99 14.47 L 332.96 24.49 L 327.97 21.97 L 322.96 24.46 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 315.918317869348 38.69214084552837)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 316px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>odometry/filtered</b></div></div></div></foreignObject><text x="316" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">odometry/filtered</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-131"><g><path d="M 240 484.72 L 240 225.7 Q 240 215.7 250 215.7 L 283.03 215.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 290.53 215.72 L 280.53 220.71 L 283.03 215.72 L 280.53 210.71 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-134"><g><path d="M 631.12 347.78 L 631.1 189 Q 631.1 179 631.03 169 L 630.08 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 630.03 14.47 L 635.09 24.44 L 630.08 21.97 L 625.09 24.5 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 618.1017614886418 44.4101760279836)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 44px; margin-left: 618px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>joint_states</b></div></div></div></foreignObject><text x="618" y="50" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">joint_states</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-135"><g><path d="M 470 10 L 470.48 207.9 Q 470.5 217.9 470.5 227.9 L 470.44 418.03" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 470.44 425.53 L 465.44 415.53 L 470.44 418.03 L 475.44 415.53 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 453.0721932435777 40.03598817157367)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 453px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>cmd_vel</b></div></div></div></foreignObject><text x="453" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">cmd_vel</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-136"><g><path d="M 582.16 347.12 L 582.2 189 Q 582.2 179 582.13 169 L 581.09 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 581.03 14.47 L 586.1 24.44 L 581.09 21.97 L 576.1 24.51 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 569.0971461347985 42.85018642894648)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 43px; margin-left: 569px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>dynamic_joint_states</b></div></div></div></foreignObject><text x="569" y="48" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">dynamic_joint_states</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-138"><g><path d="M 1209.66 730.54 L 1209.7 380.9 Q 1209.7 370.9 1209.71 360.9 L 1209.99 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1210 14.47 L 1214.99 24.48 L 1209.99 21.97 L 1204.99 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1198.040658419977 39.35417466263085)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 1198px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;"> hardware/e_stop</b></div></div></div></foreignObject><text x="1198" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end"> hardware/e_stop</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-140"><g><path d="M 1139.55 732.43 L 1139.6 380.9 Q 1139.6 370.9 1139.61 360.9 L 1139.99 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1140 14.47 L 1144.98 24.48 L 1139.99 21.97 L 1134.98 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1128.0420299914413 40.34417494560757)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1128px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>hardware/robot_driver_state</b></div></div></div></foreignObject><text x="1128" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/robot_driver_state</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-143"><g><path d="M 1173.75 730.72 L 1173.8 380.9 Q 1173.8 370.9 1173.81 360.9 L 1173.99 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1174 14.47 L 1178.99 24.47 L 1173.99 21.97 L 1168.99 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1162.0407692556278 39.434175835366155)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 1162px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;"> hardware/io_state</b></div></div></div></foreignObject><text x="1162" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end"> hardware/io_state</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-144"><g><path d="M 980.23 721.07 L 980.2 380.9 Q 980.2 370.9 980.19 360.9 L 980 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 980.23 728.57 L 975.23 718.57 L 980.23 721.07 L 985.23 718.57 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 967.0417251957298 40.1241734010298)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 967px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;">hardware/</b><b>e_stop_trigger</b></div></div></div></foreignObject><text x="967" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/e_stop_trigger</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-145"><g><path d="M 1020.7 720.62 L 1020.7 380.9 Q 1020.7 370.9 1020.68 360.9 L 1020 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1020.7 728.12 L 1015.7 718.12 L 1020.7 720.62 L 1025.7 718.12 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1007.0409632102225 39.57417226182657)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1007px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;">hardware/</b><b>e_stop_reset</b></div></div></div></foreignObject><text x="1007" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/e_stop_reset</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-148"><g><path d="M 1060.6 723.59 L 1060.6 380.9 Q 1060.6 370.9 1060.58 360.9 L 1060 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1060.6 731.09 L 1055.6 721.09 L 1060.6 723.59 L 1065.6 721.09 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1047.0407831051025 39.444172386302576)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 1047px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;">hardware/</b><b><i>&lt;hw_component&gt;</i>_enable</b></div></div></div></foreignObject><text x="1047" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/&lt;hw_component&gt;_enable</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-149"><g><rect x="665" y="210" width="240" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 242px; margin-left: 666px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">robot_state_publisher</font></div></div></div></foreignObject><text x="785" y="248" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">robot_state_publisher</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-150"><g><path d="M 830.01 210 L 830 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 830 14.47 L 835 24.47 L 830 21.97 L 825 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 818.0015500125105 41.00050021124936)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 41px; margin-left: 818px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>robot_description</b></div></div></div></foreignObject><text x="818" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">robot_description</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-151"><g><path d="M 740 210 L 740 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 740 14.47 L 745 24.47 L 740 21.97 L 735 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 728 41)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 41px; margin-left: 728px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf</b></div></div></div></foreignObject><text x="728" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-152"><g><path d="M 785 210 L 785 120.5 Q 785 110.5 785 100.5 L 785 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 785 14.47 L 790 24.47 L 785 21.97 L 780 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 771.1618442375116 41.560542301394726)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 42px; margin-left: 771px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf_static</b></div></div></div></foreignObject><text x="771" y="47" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf_static</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-153"><g><path d="M 360 185.72 L 360 108.4 Q 360 98.4 360 88.4 L 360 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 360 14.47 L 365 24.47 L 360 21.97 L 355 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 348.1597807831874 38.27752278433415)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 38px; margin-left: 348px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf</b></div></div></div></foreignObject><text x="348" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-156"><g><path d="M 1547.36 677.34 L 1600.5 677.31 Q 1610.5 677.3 1610.49 667.3 L 1610.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1610 14.47 L 1615.01 24.47 L 1610.01 21.97 L 1605.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1598.0228112826962 40.44768432687242)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1598px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">battery/battery_status</b></div></div></div></foreignObject><text x="1598" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">battery/battery_status</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-158"><g><path d="M 1173.75 730.72 L 1173.79 690 Q 1173.8 680 1183.8 680 L 1345.39 680" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1352.89 680 L 1342.89 685 L 1345.39 680 L 1342.89 675 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-159"><g><rect x="1720" y="980.72" width="160" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 1011px; margin-left: 1721px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><div style="font-size: 20px;"><span style="">Bumper Lights</span><br /></div></div></div></div></foreignObject><text x="1800" y="1016" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="16px" text-anchor="middle" font-weight="bold">Bumper Lights&#xa;</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-160"><g><rect x="1710" y="731.47" width="180" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 778px; margin-left: 1711px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">lights_driver</font></div></div></div></foreignObject><text x="1800" y="784" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">lights_driver</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-161"><g><rect x="1710" y="430" width="180" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 477px; margin-left: 1711px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">lights_controller</font></div></div></div></foreignObject><text x="1800" y="483" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">lights_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-162"><g><path d="M 1800 821.47 L 1800 962.01" fill="none" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1800 974.01 L 1794 962.01 L 1806 962.01 Z" fill="#888a85" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-165"><g><path d="M 1800 520 L 1800 719.5" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1800 727 L 1795 717 L 1800 719.5 L 1805 717 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1782 701)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 1px; height: 1px; padding-top: 701px; margin-left: 1782px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="font-size: 16px;"><font style="font-size: 16px;">lights/channel_1_frame</font></b></div></div></div></foreignObject><text x="1782" y="706" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="16px">lights/channel_1_frame</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-167"><g><path d="M 1845 520 L 1845 719.5" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1845 727 L 1840 717 L 1845 719.5 L 1850 717 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1827 701)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 1px; height: 1px; padding-top: 701px; margin-left: 1827px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="font-size: 16px;"><font style="font-size: 16px;">lights/channel_2_frame</font></b></div></div></div></foreignObject><text x="1827" y="706" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="16px">lights/channel_2_frame</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-168"><g><path d="M 1700.26 497.5 L 1502 497.5" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1707.76 497.5 L 1697.76 502.5 L 1700.26 497.5 L 1697.76 492.5 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 510px; margin-left: 1687px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: center; font-size: 18px;"><span style="background-color: initial; font-size: 18px;"><b style="font-size: 18px;"><font style="font-size: 18px;">lights/</font></b></span><span style="background-color: initial; text-align: right; font-size: 18px;"><b style="font-size: 18px;">set_animation</b></span></div></div></div></div></foreignObject><text x="1687" y="515" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">lights/set_animation</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-170"><g><path d="M 1899.74 753.98 L 1910.5 753.99 Q 1920.5 754 1920.49 744 L 1920 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1892.24 753.97 L 1902.24 748.98 L 1899.74 753.98 L 1902.23 758.98 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1907.0184835596563 37.50689698330439)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 38px; margin-left: 1907px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: center;"><b>lights/set_brightness</b><br /></div></div></div></div></foreignObject><text x="1907" y="43" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">lights/set_brightness&#xa;</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-173"><g><rect x="1312" y="314.72" width="190" height="100" rx="15" ry="15" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 367px; margin-left: 1313px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">safety_manager</font></div></div></div></foreignObject><text x="1407" y="372" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">safety_manager</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-174"><g><rect x="1393.35" y="150.57" width="180" height="70" rx="10.5" ry="10.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 188px; margin-left: 1394px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">system_monitor</font></div></div></div></foreignObject><text x="1483" y="193" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">system_monitor</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-183"><g><path d="M 1393.35 185.57 L 1350 185.59 Q 1340 185.6 1340 175.6 L 1340 150 Q 1340 140 1340 130 L 1340 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1340 14.47 L 1345 24.47 L 1340 21.97 L 1335 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1328 40.95000843486335)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 41px; margin-left: 1328px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>system_status</b></div></div></div></foreignObject><text x="1328" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">system_status</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-184"><g><path d="M 1393.35 185.57 L 1349.6 185.59 Q 1339.6 185.6 1339.6 195.6 L 1339.55 303.75" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1339.55 311.25 L 1334.56 301.25 L 1339.55 303.75 L 1344.56 301.25 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-187"><g><path d="M 1209.66 729.64 L 1209.7 374.7 Q 1209.7 364.7 1219.7 364.7 L 1300.03 364.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 364.72 L 1297.53 369.72 L 1300.03 364.72 L 1297.53 359.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-189"><g><path d="M 1173.75 730.72 L 1173.8 349.7 Q 1173.8 339.7 1183.8 339.7 L 1300.03 339.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 339.72 L 1297.53 344.72 L 1300.03 339.72 L 1297.53 334.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-190"><g><path d="M 1140 730 L 1140 324.7 Q 1140 314.7 1150 314.7 L 1300.03 314.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 314.72 L 1297.53 319.72 L 1300.03 314.72 L 1297.53 309.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-191"><g><path d="M 1060.6 723.05 L 1060.6 424.7 Q 1060.6 414.7 1070.6 414.7 L 1312 414.72" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1060.6 730.55 L 1055.6 720.55 L 1060.6 723.05 L 1065.6 720.55 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-195"><g><path d="M 1547.36 677.34 L 1556.68 677.32 Q 1566 677.3 1576 677.46 L 1600 677.84 Q 1610 678 1610 668 L 1610 374.7 Q 1610 364.7 1600 364.7 L 1513.97 364.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1506.47 364.72 L 1516.47 359.72 L 1513.97 364.72 L 1516.47 369.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-204"><g><path d="M 631.6 347.42 L 631.6 320 Q 631.6 310 631.66 300 L 631.94 250 Q 632 240 642 240 L 653.03 240" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 660.53 240 L 650.53 245 L 653.03 240 L 650.53 235 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-214"><g><path d="M 20 230.3 L 80.03 230.26" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-215"><g><path d="M 20 189.84 L 80 189.75" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/></g></g><g data-cell-id="D_R0GpuGBRVuPwS73e86-2"><g><rect x="1312" y="430" width="190" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 477px; margin-left: 1313px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">lights_manager</font></div></div></div></foreignObject><text x="1407" y="483" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">lights_manager</text></switch></g></g></g><g data-cell-id="D_R0GpuGBRVuPwS73e86-7"><g><path d="M 1547.36 677.34 L 1600 677.31 Q 1610 677.3 1610 667.3 L 1610 462.5 Q 1610 452.5 1600 452.5 L 1513.97 452.5" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1506.47 452.5 L 1516.47 447.5 L 1513.97 452.5 L 1516.47 457.5 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="D_R0GpuGBRVuPwS73e86-9"><g><path d="M 1209.66 731.08 L 1209.7 485 Q 1209.7 475 1219.7 475 L 1300.03 475" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 475 L 1297.53 480 L 1300.03 475 L 1297.53 470 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-3"><g><path d="M 1254.74 775.72 L 1467.5 775.7 Q 1477.5 775.7 1487.5 775.73 L 1710 776.47" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1247.24 775.72 L 1257.24 770.72 L 1254.74 775.72 L 1257.24 780.72 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 790px; margin-left: 1687px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><span style="forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><b style="forced-color-adjust: none;"><font style="forced-color-adjust: none; font-size: 16px;">hardware/</font></b></span><span style="forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; font-size: 16px;"><b style="forced-color-adjust: none;">led_control_enable</b></span></div></div></div></foreignObject><text x="1687" y="796" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/led_control_enable</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-6"><g><path d="M 440.02 425.38 L 440 227.9 Q 440 217.9 440 207.9 L 440 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 440 14.47 L 445 24.47 L 440 21.97 L 435 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 421.0719118764564 39.92140072145239)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 421px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf</b></div></div></div></foreignObject><text x="421" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-8"><g><path d="M 1547.36 699.84 L 1630.5 699.8 Q 1640.5 699.8 1640.49 689.8 L 1640.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1640 14.47 L 1645.01 24.47 L 1640.01 21.97 L 1635.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1628.0231543966713 41.947429886303745)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 42px; margin-left: 1628px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">battery/</b><b>charging_status</b></div></div></div></foreignObject><text x="1628" y="47" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">battery/charging_status</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-14"><g><path d="M 980.23 722.06 L 980.2 399.7 Q 980.2 389.7 990.2 389.7 L 1312 389.72" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 980.23 729.56 L 975.23 719.56 L 980.23 722.06 L 985.23 719.56 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-15"><g><rect x="950" y="850.72" width="130" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 878px; margin-left: 951px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">GPIODriver</font></div></div></div></foreignObject><text x="1015" y="883" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">GPIODriver</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-17"><g><path d="M 937.65 888.22 L 889.23 888.22" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 946.65 888.22 L 937.65 892.72 L 937.65 883.72 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 880.23 888.22 L 889.23 883.72 L 889.23 892.72 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-2"><g><path d="M 392.5 185.72 L 392.97 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 392.99 14.47 L 397.96 24.49 L 392.97 21.97 L 387.96 24.46 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 380.918317869348 38.69214084552837)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 381px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>diagnostics</b></div></div></div></foreignObject><text x="381" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-3"><g><path d="M 1547.36 722.34 L 1660.5 722.3 Q 1670.5 722.3 1670.49 712.3 L 1670.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1670 14.47 L 1675.01 24.47 L 1670.01 21.97 L 1665.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1658.02347584129 43.447193258440564)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 43px; margin-left: 1658px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: left;"><span style="background-color: initial;"><b>diagnostics</b></span></div></div></div></div></foreignObject><text x="1658" y="49" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-4"><g><path d="M 1393.35 168.07 L 1380.5 168.09 Q 1370.5 168.1 1370.47 158.1 L 1370.04 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1370.01 14.47 L 1375.05 24.46 L 1370.04 21.97 L 1365.05 24.49 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1358.0947706300105 39.98228594656824)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1358px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">diagnostics</b></div></div></div></foreignObject><text x="1358" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-6"><g><path d="M 1710 753.97 L 1680.5 753.99 Q 1670.5 754 1670.49 744 L 1670.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1670 14.47 L 1675.01 24.47 L 1670.01 21.97 L 1665.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1658.021171728833 41.506892718330505)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 42px; margin-left: 1658px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: left;"><br /></div></div></div></div></foreignObject><text x="1658" y="47" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">&#xa;</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-7"><g><path d="M 1102.5 730.72 L 1102.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1102 14.47 L 1107.01 24.47 L 1102.01 21.97 L 1097.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1090.0206207924284 39.727103790911734)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1090px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">diagnostics</b></div></div></div></foreignObject><text x="1090" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-8"><g><rect x="20" y="55" width="60" height="30" rx="4.5" ry="4.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-11"><g><rect x="0" y="5.15" width="100" height="40" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 25px; margin-left: 50px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: nowrap;"><span style="font-size: 20px; font-weight: 700; text-wrap: wrap;"><font color="#888a85">Legend:</font></span></div></div></div></foreignObject><text x="50" y="30" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="16px" text-anchor="middle">Legend:</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-12"><g><rect x="20" y="95" width="60" height="30" rx="4.5" ry="4.5" fill="#cccccc" stroke="#e6e6e6" stroke-width="6" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-14"><g><rect x="20" y="135" width="60" height="30" rx="4.5" ry="4.5" fill="#ffffff" stroke="#b0b3ac" stroke-width="2" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-18"><g><rect x="95" y="55" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 62px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Nodes</font></b></div></div></div></foreignObject><text x="97" y="80" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Nodes</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-19"><g><rect x="95" y="95" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 102px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Plugin</font></b></div></div></div></foreignObject><text x="97" y="120" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Plugin</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-20"><g><rect x="95" y="135" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 142px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Objects</font></b></div></div></div></foreignObject><text x="97" y="160" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Objects</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-21"><g><rect x="95" y="175" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 182px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Topics</font></b></div></div></div></foreignObject><text x="97" y="200" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Topics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-22"><g><rect x="95" y="215" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 222px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Services</font></b></div></div></div></foreignObject><text x="97" y="240" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Services</text></switch></g></g></g><g data-cell-id="Y5d-tTOP6FVwuIk47fc7-3"><g><path d="M 20 270 L 61.29 269.7" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 73.29 269.62 L 61.34 275.7 L 61.25 263.7 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="Y5d-tTOP6FVwuIk47fc7-4"><g><rect x="95" y="255" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 262px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Data Transfer</font></b></div></div></div></foreignObject><text x="97" y="280" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Data Trans...</text></switch></g></g></g></g></g></g><switch><g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/><a transform="translate(0,-5)" xlink:href="https://www.drawio.com/doc/faq/svg-export-text-problems" target="_blank"><text text-anchor="middle" font-size="10px" x="50%" y="100%">Text is not SVG - cannot display</text></a></switch></svg>
diff --git a/ROS_API.md b/ROS_API.md
index 915136b1..d5f6b321 100644
--- a/ROS_API.md
+++ b/ROS_API.md
@@ -73,7 +73,7 @@ Below is information about the physical robot API. For the simulation, topics an
 | 🤖🖥️⚙️ | `gps/filtered`                     | Filtered GPS position after fusing odometry data.<br/> [*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html)                                                                                                                                                                                                                         |
 | 🤖   | `hardware/e_stop`                  | Current E-stop state.<br/> [*std_msgs/Bool*](https://docs.ros2.org/latest/api/std_msgs/msg/Bool.html).                                                                                                                                                                                                                                                               |
 | 🤖   | `hardware/io_state`                | Current IO state.<br/> [*panther_msgs/IOState*](https://github.com/husarion/panther_msgs)                                                                                                                                                                                                                                                                                 |
-| 🤖🖥️  | `hardware/motor_controllers_state` | Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.<br/> [*panther_msgs/DriverState*](https://github.com/husarion/panther_msgs)                                                                                                                                                                                         |
+| 🤖🖥️  | `hardware/robot_driver_state` | Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.<br/> [*panther_msgs/RobotDriverState*](https://github.com/husarion/panther_msgs)                                                                                                                                                                                         |
 | 🤖🖥️  | `imu/data`                         | Filtered IMU data.<br/> [*sensor_msgs/Imu*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)                                                                                                                                                                                                                                                                    |
 | 🤖🖥️  | `joint_states`                     | Provides information about the state of various joints in a robotic system.<br/> [*sensor_msgs/JointState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/JointState.html)                                                                                                                                                                                             |
 | 🤖🖥️  | `lights/channel_1_frame`           | Frame to be displayed on robot Front Bumper Lights.<br/> [*sensor_msgs/Image*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html)                                                                                                                                                                                                                               |
diff --git a/panther_battery/README.md b/panther_battery/README.md
index 6b0e8b44..82ffafbe 100644
--- a/panther_battery/README.md
+++ b/panther_battery/README.md
@@ -25,7 +25,7 @@ Publishes battery state read from ADC unit for Panther version 1.2 and above, or
 #### Subscribes
 
 - `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.
+- `hardware/robot_driver_state` [*panther_msgs/DriverState*]: Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.
 
 #### Parameters
 
diff --git a/panther_battery/include/panther_battery/battery/roboteq_battery.hpp b/panther_battery/include/panther_battery/battery/roboteq_battery.hpp
index 540c5fc3..34d1d3cd 100644
--- a/panther_battery/include/panther_battery/battery/roboteq_battery.hpp
+++ b/panther_battery/include/panther_battery/battery/roboteq_battery.hpp
@@ -21,7 +21,8 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_msgs/msg/driver_state.hpp"
+#include "panther_msgs/msg/driver_state_named.hpp"
+#include "panther_msgs/msg/robot_driver_state.hpp"
 
 #include "panther_battery/battery/battery.hpp"
 #include "panther_utils/moving_average.hpp"
@@ -29,7 +30,8 @@
 namespace panther_battery
 {
 
-using DriverStateMsg = panther_msgs::msg::DriverState;
+using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
+using DriverStateNamedMsg = panther_msgs::msg::DriverStateNamed;
 
 struct RoboteqBatteryParams
 {
@@ -42,7 +44,7 @@ class RoboteqBattery : public Battery
 {
 public:
   RoboteqBattery(
-    const std::function<DriverStateMsg::SharedPtr()> & get_driver_state,
+    const std::function<RobotDriverStateMsg::SharedPtr()> & get_driver_state,
     const RoboteqBatteryParams & params);
 
   ~RoboteqBattery() {}
@@ -54,7 +56,7 @@ class RoboteqBattery : public Battery
   float GetLoadCurrent() override { return std::numeric_limits<float>::quiet_NaN(); }
 
 protected:
-  void ValidateDriverStateMsg(const rclcpp::Time & header_stamp);
+  void ValidateRobotDriverStateMsg(const rclcpp::Time & header_stamp);
 
 private:
   void UpdateBatteryMsgs(const rclcpp::Time & header_stamp);
@@ -62,14 +64,14 @@ class RoboteqBattery : public Battery
   void UpdateBatteryStateRaw();
   void UpdateChargingStatus(const rclcpp::Time & header_stamp);
   std::uint8_t GetBatteryHealth(const float voltage);
-  bool MotorControllerHeartbeatTimeout();
+  bool DriverStateHeartbeatTimeout();
 
-  std::function<DriverStateMsg::SharedPtr()> GetDriverState;
+  std::function<RobotDriverStateMsg::SharedPtr()> GetRobotDriverState;
 
   const float driver_state_timeout_;
   float voltage_raw_;
   float current_raw_;
-  DriverStateMsg::SharedPtr driver_state_;
+  RobotDriverStateMsg::SharedPtr driver_state_;
 
   std::unique_ptr<panther_utils::MovingAverage<float>> voltage_ma_;
   std::unique_ptr<panther_utils::MovingAverage<float>> current_ma_;
diff --git a/panther_battery/include/panther_battery/battery_driver_node.hpp b/panther_battery/include/panther_battery/battery_driver_node.hpp
index c13371c9..0832bc03 100644
--- a/panther_battery/include/panther_battery/battery_driver_node.hpp
+++ b/panther_battery/include/panther_battery/battery_driver_node.hpp
@@ -21,7 +21,7 @@
 #include "diagnostic_updater/diagnostic_updater.hpp"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_msgs/msg/driver_state.hpp"
+#include "panther_msgs/msg/robot_driver_state.hpp"
 
 #include "panther_battery/adc_data_reader.hpp"
 #include "panther_battery/battery/battery.hpp"
@@ -30,7 +30,7 @@
 namespace panther_battery
 {
 
-using DriverStateMsg = panther_msgs::msg::DriverState;
+using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
 
 class BatteryDriverNode : public rclcpp::Node
 {
@@ -46,7 +46,7 @@ class BatteryDriverNode : public rclcpp::Node
 
   static constexpr int kADCCurrentOffset = 625;
 
-  DriverStateMsg::SharedPtr driver_state_;
+  RobotDriverStateMsg::SharedPtr driver_state_;
 
   std::shared_ptr<ADCDataReader> adc0_reader_;
   std::shared_ptr<ADCDataReader> adc1_reader_;
@@ -54,7 +54,7 @@ class BatteryDriverNode : public rclcpp::Node
   std::shared_ptr<Battery> battery_2_;
   std::shared_ptr<BatteryPublisher> battery_publisher_;
 
-  rclcpp::Subscription<DriverStateMsg>::SharedPtr driver_state_sub_;
+  rclcpp::Subscription<RobotDriverStateMsg>::SharedPtr driver_state_sub_;
   rclcpp::TimerBase::SharedPtr battery_pub_timer_;
 
   std::shared_ptr<diagnostic_updater::Updater> diagnostic_updater_;
diff --git a/panther_battery/src/battery/roboteq_battery.cpp b/panther_battery/src/battery/roboteq_battery.cpp
index 424a06eb..0c441781 100644
--- a/panther_battery/src/battery/roboteq_battery.cpp
+++ b/panther_battery/src/battery/roboteq_battery.cpp
@@ -29,9 +29,9 @@ namespace panther_battery
 {
 
 RoboteqBattery::RoboteqBattery(
-  const std::function<DriverStateMsg::SharedPtr()> & get_driver_state,
+  const std::function<RobotDriverStateMsg::SharedPtr()> & get_driver_state,
   const RoboteqBatteryParams & params)
-: GetDriverState(get_driver_state), driver_state_timeout_(params.driver_state_timeout)
+: GetRobotDriverState(get_driver_state), driver_state_timeout_(params.driver_state_timeout)
 {
   voltage_ma_ = std::make_unique<panther_utils::MovingAverage<float>>(
     params.voltage_window_len, std::numeric_limits<float>::quiet_NaN());
@@ -43,19 +43,19 @@ bool RoboteqBattery::Present() { return true; }
 
 void RoboteqBattery::Update(const rclcpp::Time & header_stamp, const bool /* charger_connected */)
 {
-  driver_state_ = GetDriverState();
-  ValidateDriverStateMsg(header_stamp);
+  driver_state_ = GetRobotDriverState();
+  ValidateRobotDriverStateMsg(header_stamp);
 
   float voltage = 0.0f;
   float current = 0.0f;
   std::for_each(
-    driver_state_->motor_controllers.begin(), driver_state_->motor_controllers.end(),
-    [&voltage, &current](const panther_msgs::msg::MotorController & driver) {
+    driver_state_->drivers_states.begin(), driver_state_->drivers_states.end(),
+    [&voltage, &current](const DriverStateNamedMsg & driver) {
       voltage += driver.state.voltage;
       current += driver.state.current;
     });
 
-  voltage_raw_ = voltage / driver_state_->motor_controllers.size();
+  voltage_raw_ = voltage / driver_state_->drivers_states.size();
   current_raw_ = current;
   voltage_ma_->Roll(voltage_raw_);
   current_ma_->Roll(current_raw_);
@@ -72,7 +72,7 @@ void RoboteqBattery::Reset(const rclcpp::Time & header_stamp)
   SetErrorMsg("");
 }
 
-void RoboteqBattery::ValidateDriverStateMsg(const rclcpp::Time & header_stamp)
+void RoboteqBattery::ValidateRobotDriverStateMsg(const rclcpp::Time & header_stamp)
 {
   if (!driver_state_) {
     throw std::runtime_error("Waiting for driver state message to arrive.");
@@ -83,7 +83,7 @@ void RoboteqBattery::ValidateDriverStateMsg(const rclcpp::Time & header_stamp)
     throw std::runtime_error("Driver state message timeout.");
   }
 
-  if (MotorControllerHeartbeatTimeout()) {
+  if (DriverStateHeartbeatTimeout()) {
     throw std::runtime_error("Motor controller heartbeat timeout error.");
   }
 }
@@ -150,13 +150,11 @@ std::uint8_t RoboteqBattery::GetBatteryHealth(const float voltage)
   }
 }
 
-bool RoboteqBattery::MotorControllerHeartbeatTimeout()
+bool RoboteqBattery::DriverStateHeartbeatTimeout()
 {
   return std::any_of(
-    driver_state_->motor_controllers.begin(), driver_state_->motor_controllers.end(),
-    [](const panther_msgs::msg::MotorController & driver) {
-      return driver.state.heartbeat_timeout;
-    });
+    driver_state_->drivers_states.begin(), driver_state_->drivers_states.end(),
+    [](const DriverStateNamedMsg & driver) { return driver.state.heartbeat_timeout; });
 }
 
 }  // namespace panther_battery
diff --git a/panther_battery/src/battery_driver_node.cpp b/panther_battery/src/battery_driver_node.cpp
index d1ff8acd..e24ff658 100644
--- a/panther_battery/src/battery_driver_node.cpp
+++ b/panther_battery/src/battery_driver_node.cpp
@@ -140,9 +140,9 @@ void BatteryDriverNode::InitializeWithRoboteqBattery()
     static_cast<std::size_t>(this->get_parameter("ma_window_len/current").as_int()),
   };
 
-  driver_state_sub_ = this->create_subscription<DriverStateMsg>(
-    "hardware/motor_controllers_state", 5,
-    [&](const DriverStateMsg::SharedPtr msg) { driver_state_ = msg; });
+  driver_state_sub_ = this->create_subscription<RobotDriverStateMsg>(
+    "hardware/robot_driver_state", 5,
+    [&](const RobotDriverStateMsg::SharedPtr msg) { driver_state_ = msg; });
 
   battery_1_ = std::make_shared<RoboteqBattery>([&]() { return driver_state_; }, battery_params);
 
diff --git a/panther_battery/test/battery/test_roboteq_battery.cpp b/panther_battery/test/battery/test_roboteq_battery.cpp
index 4d15f541..c8da21e2 100644
--- a/panther_battery/test/battery/test_roboteq_battery.cpp
+++ b/panther_battery/test/battery/test_roboteq_battery.cpp
@@ -24,27 +24,28 @@
 
 #include "sensor_msgs/msg/battery_state.hpp"
 
-#include "panther_msgs/msg/driver_state.hpp"
+#include "panther_msgs/msg/driver_state_named.hpp"
+#include "panther_msgs/msg/robot_driver_state.hpp"
 
 #include "panther_battery/battery/roboteq_battery.hpp"
 #include "panther_utils/test/test_utils.hpp"
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
-using DriverStateMsg = panther_msgs::msg::DriverState;
+using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
 
 class RoboteqBatteryWrapper : public panther_battery::RoboteqBattery
 {
 public:
   RoboteqBatteryWrapper(
-    const std::function<DriverStateMsg::SharedPtr()> & get_driver_state,
+    const std::function<RobotDriverStateMsg::SharedPtr()> & get_driver_state,
     const panther_battery::RoboteqBatteryParams & params)
   : RoboteqBattery(get_driver_state, params)
   {
   }
 
-  void ValidateDriverStateMsg(const rclcpp::Time & header_stamp)
+  void ValidateRobotDriverStateMsg(const rclcpp::Time & header_stamp)
   {
-    return RoboteqBattery::ValidateDriverStateMsg(header_stamp);
+    return RoboteqBattery::ValidateRobotDriverStateMsg(header_stamp);
   }
 };
 
@@ -63,34 +64,34 @@ class TestRoboteqBattery : public testing::Test
     const float expected_voltage, const float expected_current, const float expected_percentage,
     const std::uint8_t power_supply_status, const std::uint8_t power_supply_health);
 
-  static constexpr float kDriverStateTimeout = 0.2;
+  static constexpr float kRobotDriverStateTimeout = 0.2;
 
   std::unique_ptr<RoboteqBatteryWrapper> battery_;
   BatteryStateMsg battery_state_;
-  DriverStateMsg::SharedPtr driver_state_;
+  RobotDriverStateMsg::SharedPtr driver_state_;
 };
 
 TestRoboteqBattery::TestRoboteqBattery()
 {
-  const panther_battery::RoboteqBatteryParams params = {kDriverStateTimeout, 10, 10};
+  const panther_battery::RoboteqBatteryParams params = {kRobotDriverStateTimeout, 10, 10};
   battery_ = std::make_unique<RoboteqBatteryWrapper>([&]() { return driver_state_; }, params);
 }
 
 void TestRoboteqBattery::UpdateBattery(const float voltage, const float current)
 {
   if (!driver_state_) {
-    driver_state_ = std::make_shared<DriverStateMsg>();
-    driver_state_->motor_controllers.push_back(panther_msgs::msg::MotorController());
-    driver_state_->motor_controllers.push_back(panther_msgs::msg::MotorController());
+    driver_state_ = std::make_shared<RobotDriverStateMsg>();
+    driver_state_->drivers_states.push_back(panther_msgs::msg::DriverStateNamed());
+    driver_state_->drivers_states.push_back(panther_msgs::msg::DriverStateNamed());
   }
 
   auto stamp = rclcpp::Time(0, 0, RCL_ROS_TIME);
 
   driver_state_->header.stamp = stamp;
-  driver_state_->motor_controllers.at(0).state.voltage = voltage;
-  driver_state_->motor_controllers.at(1).state.voltage = voltage;
-  driver_state_->motor_controllers.at(0).state.current = current;
-  driver_state_->motor_controllers.at(1).state.current = current;
+  driver_state_->drivers_states.at(0).state.voltage = voltage;
+  driver_state_->drivers_states.at(1).state.voltage = voltage;
+  driver_state_->drivers_states.at(0).state.current = current;
+  driver_state_->drivers_states.at(1).state.current = current;
 
   battery_->Update(stamp, false);
   battery_state_ = battery_->GetBatteryMsg();
@@ -222,31 +223,31 @@ TEST_F(TestRoboteqBattery, GetErrorMsg)
   EXPECT_NE("", battery_->GetErrorMsg());
 }
 
-TEST_F(TestRoboteqBattery, ValidateDriverStateMsg)
+TEST_F(TestRoboteqBattery, ValidateRobotDriverStateMsg)
 {
   auto stamp = rclcpp::Time(0, 0, RCL_ROS_TIME);
 
   // Check empty driver_state msg
-  EXPECT_THROW(battery_->ValidateDriverStateMsg(stamp), std::runtime_error);
+  EXPECT_THROW(battery_->ValidateRobotDriverStateMsg(stamp), std::runtime_error);
 
   UpdateBattery(35.0f, 0.1f);
-  EXPECT_NO_THROW(battery_->ValidateDriverStateMsg(stamp));
+  EXPECT_NO_THROW(battery_->ValidateRobotDriverStateMsg(stamp));
 
   // Check timeout
-  const std::uint32_t timeout_nanoseconds = (kDriverStateTimeout + 0.05) * 1e9;
+  const std::uint32_t timeout_nanoseconds = (kRobotDriverStateTimeout + 0.05) * 1e9;
   stamp = rclcpp::Time(0, timeout_nanoseconds, RCL_ROS_TIME);
-  EXPECT_THROW(battery_->ValidateDriverStateMsg(stamp), std::runtime_error);
+  EXPECT_THROW(battery_->ValidateRobotDriverStateMsg(stamp), std::runtime_error);
 
   // Reset error
   stamp = rclcpp::Time(0, 0, RCL_ROS_TIME);
-  EXPECT_NO_THROW(battery_->ValidateDriverStateMsg(stamp));
+  EXPECT_NO_THROW(battery_->ValidateRobotDriverStateMsg(stamp));
 
   // Check heartbeat timeout error throw
-  driver_state_->motor_controllers.at(0).state.heartbeat_timeout = true;
-  EXPECT_THROW(battery_->ValidateDriverStateMsg(stamp), std::runtime_error);
-  driver_state_->motor_controllers.at(0).state.heartbeat_timeout = false;
-  driver_state_->motor_controllers.at(1).state.heartbeat_timeout = true;
-  EXPECT_THROW(battery_->ValidateDriverStateMsg(stamp), std::runtime_error);
+  driver_state_->drivers_states.at(0).state.heartbeat_timeout = true;
+  EXPECT_THROW(battery_->ValidateRobotDriverStateMsg(stamp), std::runtime_error);
+  driver_state_->drivers_states.at(0).state.heartbeat_timeout = false;
+  driver_state_->drivers_states.at(1).state.heartbeat_timeout = true;
+  EXPECT_THROW(battery_->ValidateRobotDriverStateMsg(stamp), std::runtime_error);
 }
 
 int main(int argc, char ** argv)
diff --git a/panther_battery/test/test_battery_driver_node_adc_single.cpp b/panther_battery/test/test_battery_driver_node_adc_single.cpp
index 6ddd48b3..21f6995f 100644
--- a/panther_battery/test/test_battery_driver_node_adc_single.cpp
+++ b/panther_battery/test/test_battery_driver_node_adc_single.cpp
@@ -112,7 +112,7 @@ TEST_F(TestBatteryNodeADCSingle, RoboteqInitOnADCFail)
   EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_STATUS_UNKNOWN, battery_state_->power_supply_status);
 
   // Publish driver state that should update the battery message
-  DriverStateMsg driver_state;
+  RobotDriverStateMsg driver_state;
   driver_state.header.stamp = battery_driver_node_->get_clock()->now();
   driver_state_pub_->publish(driver_state);
 
diff --git a/panther_battery/test/test_battery_driver_node_roboteq.cpp b/panther_battery/test/test_battery_driver_node_roboteq.cpp
index 757faaa0..ae1fa1db 100644
--- a/panther_battery/test/test_battery_driver_node_roboteq.cpp
+++ b/panther_battery/test/test_battery_driver_node_roboteq.cpp
@@ -44,14 +44,14 @@ TEST_F(TestBatteryNodeRoboteq, BatteryValues)
   EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_STATUS_UNKNOWN, battery_state_->power_supply_status);
   EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_HEALTH_UNKNOWN, battery_state_->power_supply_health);
 
-  panther_msgs::msg::MotorController motor_controller;
+  panther_msgs::msg::DriverStateNamed motor_controller;
   motor_controller.state.voltage = 35.0f;
   motor_controller.state.current = 0.1f;
 
-  DriverStateMsg driver_state;
+  RobotDriverStateMsg driver_state;
   driver_state.header.stamp = battery_driver_node_->get_clock()->now();
-  driver_state.motor_controllers.push_back(motor_controller);
-  driver_state.motor_controllers.push_back(motor_controller);
+  driver_state.drivers_states.push_back(motor_controller);
+  driver_state.drivers_states.push_back(motor_controller);
   driver_state_pub_->publish(driver_state);
 
   ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
@@ -84,14 +84,14 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout)
   EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_HEALTH_UNKNOWN, battery_state_->power_supply_health);
 
   // Publish some values
-  panther_msgs::msg::MotorController motor_controller;
+  panther_msgs::msg::DriverStateNamed motor_controller;
   motor_controller.state.voltage = 35.0f;
   motor_controller.state.current = 0.1f;
 
-  DriverStateMsg driver_state;
+  RobotDriverStateMsg driver_state;
   driver_state.header.stamp = battery_driver_node_->get_clock()->now();
-  driver_state.motor_controllers.push_back(motor_controller);
-  driver_state.motor_controllers.push_back(motor_controller);
+  driver_state.drivers_states.push_back(motor_controller);
+  driver_state.drivers_states.push_back(motor_controller);
   driver_state_pub_->publish(driver_state);
 
   ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
diff --git a/panther_battery/test/utils/test_battery_driver_node.hpp b/panther_battery/test/utils/test_battery_driver_node.hpp
index 9b2385b2..4ada0103 100644
--- a/panther_battery/test/utils/test_battery_driver_node.hpp
+++ b/panther_battery/test/utils/test_battery_driver_node.hpp
@@ -28,13 +28,13 @@
 
 #include "sensor_msgs/msg/battery_state.hpp"
 
-#include "panther_msgs/msg/driver_state.hpp"
 #include "panther_msgs/msg/io_state.hpp"
+#include "panther_msgs/msg/robot_driver_state.hpp"
 
 #include "panther_battery/battery_driver_node.hpp"
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
-using DriverStateMsg = panther_msgs::msg::DriverState;
+using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
 using IOStateMsg = panther_msgs::msg::IOState;
 
 class TestBatteryNode : public testing::Test
@@ -60,7 +60,7 @@ class TestBatteryNode : public testing::Test
   rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_1_sub_;
   rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_2_sub_;
   rclcpp::Publisher<IOStateMsg>::SharedPtr io_state_pub_;
-  rclcpp::Publisher<DriverStateMsg>::SharedPtr driver_state_pub_;
+  rclcpp::Publisher<RobotDriverStateMsg>::SharedPtr driver_state_pub_;
 };
 
 TestBatteryNode::TestBatteryNode(const float panther_version, const bool dual_battery)
@@ -119,8 +119,8 @@ TestBatteryNode::TestBatteryNode(const float panther_version, const bool dual_ba
 
   io_state_pub_ = battery_driver_node_->create_publisher<IOStateMsg>(
     "hardware/io_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
-  driver_state_pub_ = battery_driver_node_->create_publisher<DriverStateMsg>(
-    "hardware/motor_controllers_state", 10);
+  driver_state_pub_ = battery_driver_node_->create_publisher<RobotDriverStateMsg>(
+    "hardware/robot_driver_state", 10);
 }
 
 TestBatteryNode::~TestBatteryNode()
diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py
index 92c9b7a6..a2212948 100644
--- a/panther_controller/launch/controller.launch.py
+++ b/panther_controller/launch/controller.launch.py
@@ -186,7 +186,7 @@ def generate_launch_description():
             ("hardware_controller/fan_enable", "hardware/fan_enable"),
             ("hardware_controller/io_state", "hardware/io_state"),
             ("hardware_controller/led_control_enable", "hardware/led_control_enable"),
-            ("hardware_controller/motor_controllers_state", "hardware/motor_controllers_state"),
+            ("hardware_controller/robot_driver_state", "hardware/robot_driver_state"),
             ("hardware_controller/motor_power_enable", "hardware/motor_power_enable"),
             ("imu_broadcaster/imu", "imu/data"),
             ("imu_broadcaster/transition_event", "_imu_broadcaster/transition_event"),
diff --git a/panther_hardware_interfaces/README.md b/panther_hardware_interfaces/README.md
index 99b79859..d2c45648 100644
--- a/panther_hardware_interfaces/README.md
+++ b/panther_hardware_interfaces/README.md
@@ -15,7 +15,7 @@ Plugin responsible for communicating with engine controllers via the CAN bus, pr
 - `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Panther system diagnostic messages.
 - `hardware/e_stop` [*std_msgs/Bool*]: Current E-stop state.
 - `hardware/io_state` [*panther_msgs/IOState*]: Current IO state.
-- `hardware/motor_controllers_state` [*panther_msgs/DriverState*]: Current motor controllers' state and error flags.
+- `hardware/robot_driver_state` [*panther_msgs/DriverState*]: Current motor controllers' state and error flags.
 
 #### Service Servers
 
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp
index a117737f..ca04943d 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp
@@ -30,9 +30,9 @@
 #include "std_srvs/srv/set_bool.hpp"
 #include "std_srvs/srv/trigger.hpp"
 
-#include "panther_msgs/msg/driver_state.hpp"
+#include "panther_msgs/msg/driver_state_named.hpp"
 #include "panther_msgs/msg/io_state.hpp"
-#include "panther_msgs/msg/motor_controller.hpp"
+#include "panther_msgs/msg/robot_driver_state.hpp"
 
 #include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp"
 #include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp"
@@ -43,9 +43,9 @@ namespace panther_hardware_interfaces
 {
 
 using BoolMsg = std_msgs::msg::Bool;
-using DriverStateMsg = panther_msgs::msg::DriverState;
+using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
 using IOStateMsg = panther_msgs::msg::IOState;
-using MotorControllerMsg = panther_msgs::msg::MotorController;
+using DriverStateNamedMsg = panther_msgs::msg::DriverStateNamed;
 using SetBoolSrv = std_srvs::srv::SetBool;
 using TriggerSrv = std_srvs::srv::Trigger;
 
@@ -238,7 +238,7 @@ class PantherSystemRosInterface
 
   void PublishEStopStateMsg(const bool e_stop);
   void PublishEStopStateIfChanged(const bool e_stop);
-  void PublishDriverState();
+  void PublishRobotDriverState();
   void InitializeAndPublishIOStateMsg(const std::unordered_map<GPIOPin, bool> & io_state);
   void PublishIOState(const GPIOInfo & gpio_info);
 
@@ -271,18 +271,18 @@ class PantherSystemRosInterface
   rclcpp::CallbackGroup::SharedPtr GetOrCreateNodeCallbackGroup(
     const unsigned group_id, rclcpp::CallbackGroupType callback_group_type);
 
-  void InitializeDriverStateMsg();
+  void InitializeRobotDriverStateMsg();
 
-  MotorControllerMsg & GetMotorControllerByName(
-    DriverStateMsg & driver_state, const std::string & name);
+  DriverStateNamedMsg & GetDriverStateByName(
+    RobotDriverStateMsg & robot_driver_state, const std::string & name);
 
   rclcpp::Node::SharedPtr node_;
   std::unordered_map<unsigned, rclcpp::CallbackGroup::SharedPtr> callback_groups_;
   rclcpp::executors::MultiThreadedExecutor::UniquePtr executor_;
   std::thread executor_thread_;
 
-  rclcpp::Publisher<DriverStateMsg>::SharedPtr driver_state_publisher_;
-  std::unique_ptr<realtime_tools::RealtimePublisher<DriverStateMsg>>
+  rclcpp::Publisher<RobotDriverStateMsg>::SharedPtr driver_state_publisher_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<RobotDriverStateMsg>>
     realtime_driver_state_publisher_;
 
   rclcpp::Publisher<IOStateMsg>::SharedPtr io_state_publisher_;
diff --git a/panther_hardware_interfaces/src/panther_system/panther_system.cpp b/panther_hardware_interfaces/src/panther_system/panther_system.cpp
index 05c4c6e9..4134d9c3 100644
--- a/panther_hardware_interfaces/src/panther_system/panther_system.cpp
+++ b/panther_hardware_interfaces/src/panther_system/panther_system.cpp
@@ -251,7 +251,7 @@ return_type PantherSystem::read(const rclcpp::Time & time, const rclcpp::Duratio
   if (time >= next_driver_state_update_time_) {
     UpdateDriverState();
     UpdateDriverStateMsg();
-    panther_system_ros_interface_->PublishDriverState();
+    panther_system_ros_interface_->PublishRobotDriverState();
     next_driver_state_update_time_ = time + driver_states_update_period_;
   }
 
diff --git a/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp b/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp
index 6766d85f..22a02752 100644
--- a/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp
+++ b/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp
@@ -82,11 +82,12 @@ PantherSystemRosInterface::PantherSystemRosInterface(
 
   executor_thread_ = std::thread([this]() { executor_->spin(); });
 
-  driver_state_publisher_ = node_->create_publisher<DriverStateMsg>("~/motor_controllers_state", 5);
+  driver_state_publisher_ = node_->create_publisher<RobotDriverStateMsg>("~/robot_driver_state", 5);
   realtime_driver_state_publisher_ =
-    std::make_unique<realtime_tools::RealtimePublisher<DriverStateMsg>>(driver_state_publisher_);
+    std::make_unique<realtime_tools::RealtimePublisher<RobotDriverStateMsg>>(
+      driver_state_publisher_);
 
-  InitializeDriverStateMsg();
+  InitializeRobotDriverStateMsg();
 
   io_state_publisher_ = node_->create_publisher<IOStateMsg>(
     "~/io_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
@@ -131,73 +132,64 @@ void PantherSystemRosInterface::UpdateMsgErrorFlags(
   const RoboteqData & front, const RoboteqData & rear)
 {
   auto & driver_state = realtime_driver_state_publisher_->msg_;
-  auto & front_motor_controller = GetMotorControllerByName(
-    driver_state, MotorControllerMsg::NAME_FRONT);
-  auto & rear_motor_controller = GetMotorControllerByName(
-    driver_state, MotorControllerMsg::NAME_REAR);
+  auto & front_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_FRONT);
+  auto & rear_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_REAR);
 
   driver_state.header.stamp = node_->get_clock()->now();
 
-  front_motor_controller.state.fault_flag = front.GetFaultFlag().GetMessage();
-  front_motor_controller.state.script_flag = front.GetScriptFlag().GetMessage();
-  front_motor_controller.state.left_motor_runtime_error = front.GetLeftRuntimeError().GetMessage();
-  front_motor_controller.state.right_motor_runtime_error =
-    front.GetRightRuntimeError().GetMessage();
+  front_driver_state.state.fault_flag = front.GetFaultFlag().GetMessage();
+  front_driver_state.state.script_flag = front.GetScriptFlag().GetMessage();
+  front_driver_state.state.left_motor_runtime_error = front.GetLeftRuntimeError().GetMessage();
+  front_driver_state.state.right_motor_runtime_error = front.GetRightRuntimeError().GetMessage();
 
-  rear_motor_controller.state.fault_flag = rear.GetFaultFlag().GetMessage();
-  rear_motor_controller.state.script_flag = rear.GetScriptFlag().GetMessage();
-  rear_motor_controller.state.left_motor_runtime_error = rear.GetLeftRuntimeError().GetMessage();
-  rear_motor_controller.state.right_motor_runtime_error = rear.GetRightRuntimeError().GetMessage();
+  rear_driver_state.state.fault_flag = rear.GetFaultFlag().GetMessage();
+  rear_driver_state.state.script_flag = rear.GetScriptFlag().GetMessage();
+  rear_driver_state.state.left_motor_runtime_error = rear.GetLeftRuntimeError().GetMessage();
+  rear_driver_state.state.right_motor_runtime_error = rear.GetRightRuntimeError().GetMessage();
 }
 
 void PantherSystemRosInterface::UpdateMsgDriversStates(
   const DriverState & front, const DriverState & rear)
 {
   auto & driver_state = realtime_driver_state_publisher_->msg_;
-  auto & front_motor_controller = GetMotorControllerByName(
-    driver_state, MotorControllerMsg::NAME_FRONT);
-  auto & rear_motor_controller = GetMotorControllerByName(
-    driver_state, MotorControllerMsg::NAME_REAR);
-
-  front_motor_controller.state.voltage = front.GetVoltage();
-  front_motor_controller.state.current = front.GetCurrent();
-  front_motor_controller.state.temperature = front.GetTemperature();
-  front_motor_controller.state.heatsink_temperature = front.GetHeatsinkTemperature();
-
-  rear_motor_controller.state.voltage = rear.GetVoltage();
-  rear_motor_controller.state.current = rear.GetCurrent();
-  rear_motor_controller.state.temperature = rear.GetTemperature();
-  rear_motor_controller.state.heatsink_temperature = rear.GetHeatsinkTemperature();
+  auto & front_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_FRONT);
+  auto & rear_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_REAR);
+
+  front_driver_state.state.voltage = front.GetVoltage();
+  front_driver_state.state.current = front.GetCurrent();
+  front_driver_state.state.temperature = front.GetTemperature();
+  front_driver_state.state.heatsink_temperature = front.GetHeatsinkTemperature();
+
+  rear_driver_state.state.voltage = rear.GetVoltage();
+  rear_driver_state.state.current = rear.GetCurrent();
+  rear_driver_state.state.temperature = rear.GetTemperature();
+  rear_driver_state.state.heatsink_temperature = rear.GetHeatsinkTemperature();
 }
 
 void PantherSystemRosInterface::UpdateMsgErrors(const CANErrors & can_errors)
 {
   auto & driver_state = realtime_driver_state_publisher_->msg_;
-  auto & front_motor_controller = GetMotorControllerByName(
-    driver_state, MotorControllerMsg::NAME_FRONT);
-  auto & rear_motor_controller = GetMotorControllerByName(
-    driver_state, MotorControllerMsg::NAME_REAR);
+  auto & front_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_FRONT);
+  auto & rear_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_REAR);
 
   driver_state.error = can_errors.error;
   driver_state.write_pdo_cmds_error = can_errors.write_pdo_cmds_error;
   driver_state.read_pdo_motor_states_error = can_errors.read_pdo_motor_states_error;
   driver_state.read_pdo_driver_state_error = can_errors.read_pdo_driver_state_error;
 
-  front_motor_controller.state.motor_states_data_timed_out =
+  front_driver_state.state.motor_states_data_timed_out =
     can_errors.front_motor_states_data_timed_out;
-  rear_motor_controller.state.motor_states_data_timed_out =
-    can_errors.rear_motor_states_data_timed_out;
+  rear_driver_state.state.motor_states_data_timed_out = can_errors.rear_motor_states_data_timed_out;
 
-  front_motor_controller.state.driver_state_data_timed_out =
+  front_driver_state.state.driver_state_data_timed_out =
     can_errors.front_driver_state_data_timed_out;
-  rear_motor_controller.state.driver_state_data_timed_out =
-    can_errors.rear_driver_state_data_timed_out;
+  rear_driver_state.state.driver_state_data_timed_out = can_errors.rear_driver_state_data_timed_out;
 
-  front_motor_controller.state.can_error = can_errors.front_can_error;
-  rear_motor_controller.state.can_error = can_errors.rear_can_error;
+  front_driver_state.state.can_error = can_errors.front_can_error;
+  rear_driver_state.state.can_error = can_errors.rear_can_error;
 
-  front_motor_controller.state.heartbeat_timeout = can_errors.front_heartbeat_timeout;
-  rear_motor_controller.state.heartbeat_timeout = can_errors.rear_heartbeat_timeout;
+  front_driver_state.state.heartbeat_timeout = can_errors.front_heartbeat_timeout;
+  rear_driver_state.state.heartbeat_timeout = can_errors.rear_heartbeat_timeout;
 }
 
 void PantherSystemRosInterface::PublishEStopStateMsg(const bool e_stop)
@@ -215,7 +207,7 @@ void PantherSystemRosInterface::PublishEStopStateIfChanged(const bool e_stop)
   }
 }
 
-void PantherSystemRosInterface::PublishDriverState()
+void PantherSystemRosInterface::PublishRobotDriverState()
 {
   if (realtime_driver_state_publisher_->trylock()) {
     realtime_driver_state_publisher_->unlockAndPublish();
@@ -309,29 +301,29 @@ rclcpp::CallbackGroup::SharedPtr PantherSystemRosInterface::GetOrCreateNodeCallb
   return callback_group;
 }
 
-void PantherSystemRosInterface::InitializeDriverStateMsg()
+void PantherSystemRosInterface::InitializeRobotDriverStateMsg()
 {
-  MotorControllerMsg front_motor_controller;
-  MotorControllerMsg rear_motor_controller;
-  front_motor_controller.name = MotorControllerMsg::NAME_FRONT;
-  rear_motor_controller.name = MotorControllerMsg::NAME_REAR;
+  DriverStateNamedMsg front_driver_state;
+  DriverStateNamedMsg rear_driver_state;
+  front_driver_state.name = DriverStateNamedMsg::NAME_FRONT;
+  rear_driver_state.name = DriverStateNamedMsg::NAME_REAR;
 
   auto & driver_state = realtime_driver_state_publisher_->msg_;
-  driver_state.motor_controllers.push_back(front_motor_controller);
-  driver_state.motor_controllers.push_back(rear_motor_controller);
+  driver_state.drivers_states.push_back(front_driver_state);
+  driver_state.drivers_states.push_back(rear_driver_state);
 }
 
-MotorControllerMsg & PantherSystemRosInterface::GetMotorControllerByName(
-  DriverStateMsg & driver_state, const std::string & name)
+DriverStateNamedMsg & PantherSystemRosInterface::GetDriverStateByName(
+  RobotDriverStateMsg & robot_driver_state, const std::string & name)
 {
-  auto & motor_controllers = driver_state.motor_controllers;
+  auto & drivers_states = robot_driver_state.drivers_states;
 
   auto it = std::find_if(
-    motor_controllers.begin(), motor_controllers.end(),
-    [&name](const MotorControllerMsg & msg) { return msg.name == name; });
+    drivers_states.begin(), drivers_states.end(),
+    [&name](const DriverStateNamedMsg & msg) { return msg.name == name; });
 
-  if (it == motor_controllers.end()) {
-    throw std::runtime_error("Motor controller with name '" + name + "' not found.");
+  if (it == drivers_states.end()) {
+    throw std::runtime_error("Driver with name '" + name + "' not found.");
   }
 
   return *it;
diff --git a/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp b/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp
index c4d7d50b..635715fb 100644
--- a/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp
+++ b/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp
@@ -28,13 +28,15 @@
 
 #include <lifecycle_msgs/msg/state.hpp>
 
-#include <panther_msgs/msg/driver_state.hpp>
+#include <panther_msgs/msg/robot_driver_state.hpp>
 
 #include <panther_utils/test/ros_test_utils.hpp>
 
 #include "utils/panther_system_test_utils.hpp"
 #include "utils/roboteqs_mock.hpp"
 
+using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
+
 class TestPantherSystem : public ::testing::Test
 {
 public:
@@ -427,11 +429,11 @@ TEST_F(TestPantherSystem, ReadOtherRoboteqParamsPantherSystem)
 
   pth_test_.ConfigureActivatePantherSystem();
 
-  panther_msgs::msg::DriverState::SharedPtr state_msg;
+  RobotDriverStateMsg::SharedPtr state_msg;
   unsigned state_msg_count = 0;
-  auto sub = node->create_subscription<panther_msgs::msg::DriverState>(
-    panther_hardware_interfaces_test::kMotorControllersStateTopic, rclcpp::SensorDataQoS(),
-    [&](const panther_msgs::msg::DriverState::SharedPtr msg) {
+  auto sub = node->create_subscription<RobotDriverStateMsg>(
+    panther_hardware_interfaces_test::kRobotDriverStateTopic, rclcpp::SensorDataQoS(),
+    [&](const RobotDriverStateMsg::SharedPtr msg) {
       state_msg = msg;
       ++state_msg_count;
     });
@@ -451,28 +453,26 @@ TEST_F(TestPantherSystem, ReadOtherRoboteqParamsPantherSystem)
 
   ASSERT_TRUE(state_msg);
 
-  ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->motor_controllers.at(0).state.temperature), f_temp);
-  ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->motor_controllers.at(1).state.temperature), r_temp);
+  ASSERT_EQ(static_cast<std::int16_t>(state_msg->drivers_states.at(0).state.temperature), f_temp);
+  ASSERT_EQ(static_cast<std::int16_t>(state_msg->drivers_states.at(1).state.temperature), r_temp);
 
   ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->motor_controllers.at(0).state.heatsink_temperature),
+    static_cast<std::int16_t>(state_msg->drivers_states.at(0).state.heatsink_temperature),
     f_heatsink_temp);
   ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->motor_controllers.at(1).state.heatsink_temperature),
+    static_cast<std::int16_t>(state_msg->drivers_states.at(1).state.heatsink_temperature),
     r_heatsink_temp);
 
   ASSERT_EQ(
-    static_cast<std::uint16_t>(state_msg->motor_controllers.at(0).state.voltage * 10.0), f_volt);
+    static_cast<std::uint16_t>(state_msg->drivers_states.at(0).state.voltage * 10.0), f_volt);
   ASSERT_EQ(
-    static_cast<std::uint16_t>(state_msg->motor_controllers.at(1).state.voltage * 10.0), r_volt);
+    static_cast<std::uint16_t>(state_msg->drivers_states.at(1).state.voltage * 10.0), r_volt);
 
   ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->motor_controllers.at(0).state.current * 10.0),
+    static_cast<std::int16_t>(state_msg->drivers_states.at(0).state.current * 10.0),
     (f_battery_current_1 + f_battery_current_2));
   ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->motor_controllers.at(1).state.current * 10.0),
+    static_cast<std::int16_t>(state_msg->drivers_states.at(1).state.current * 10.0),
     (r_battery_current_1 + r_battery_current_2));
 
   pth_test_.ShutdownPantherSystem();
@@ -492,10 +492,10 @@ TEST_F(TestPantherSystem, EncoderDisconnectedPantherSystem)
 
   pth_test_.ConfigureActivatePantherSystem();
 
-  panther_msgs::msg::DriverState::SharedPtr state_msg;
-  auto sub = node->create_subscription<panther_msgs::msg::DriverState>(
-    panther_hardware_interfaces_test::kMotorControllersStateTopic, rclcpp::SensorDataQoS(),
-    [&](const panther_msgs::msg::DriverState::SharedPtr msg) { state_msg = msg; });
+  RobotDriverStateMsg::SharedPtr state_msg;
+  auto sub = node->create_subscription<RobotDriverStateMsg>(
+    panther_hardware_interfaces_test::kRobotDriverStateTopic, rclcpp::SensorDataQoS(),
+    [&](const RobotDriverStateMsg::SharedPtr msg) { state_msg = msg; });
 
   std::this_thread::sleep_for(std::chrono::seconds(2));
 
@@ -505,7 +505,7 @@ TEST_F(TestPantherSystem, EncoderDisconnectedPantherSystem)
   pth_test_.GetResourceManager()->read(TIME, PERIOD);
 
   ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5)));
-  ASSERT_TRUE(state_msg->motor_controllers.at(0).state.script_flag.encoder_disconnected);
+  ASSERT_TRUE(state_msg->drivers_states.at(0).state.script_flag.encoder_disconnected);
 
   // writing should be blocked - error
 
@@ -718,10 +718,10 @@ TEST(TestPantherSystemOthers, WrongOrderURDF)
 //   rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("hardware_interface_test_node");
 //   pth_test_.ConfigureActivatePantherSystem();
 
-//   panther_msgs::msg::DriverState::SharedPtr state_msg;
-//   auto sub = node->create_subscription<panther_msgs::msg::DriverState>(
-//     panther_hardware_interfaces_test::kMotorControllersStateTopic, rclcpp::SensorDataQoS(),
-//     [&](const panther_msgs::msg::DriverState::SharedPtr msg) { state_msg = msg; });
+//   RobotDriverStateMsg::SharedPtr state_msg;
+//   auto sub = node->create_subscription<RobotDriverStateMsg>(
+//     panther_hardware_interfaces_test::kRobotDriverStateTopic, rclcpp::SensorDataQoS(),
+//     [&](const RobotDriverStateMsg::SharedPtr msg) { state_msg = msg; });
 
 //   std::this_thread::sleep_for(std::chrono::seconds(2));
 
diff --git a/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp b/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
index 75fbd40f..9412242a 100644
--- a/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
+++ b/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
@@ -29,6 +29,7 @@
 
 #include "utils/test_constants.hpp"
 
+using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
 class TestPantherSystemRosInterface : public ::testing::Test
 {
 public:
@@ -38,9 +39,9 @@ class TestPantherSystemRosInterface : public ::testing::Test
 
     test_node_ = std::make_shared<rclcpp::Node>("test_panther_system_node");
 
-    driver_state_sub_ = test_node_->create_subscription<panther_msgs::msg::DriverState>(
-      panther_hardware_interfaces_test::kMotorControllersStateTopic, rclcpp::SensorDataQoS(),
-      [&](const panther_msgs::msg::DriverState::SharedPtr msg) { driver_state_msg_ = msg; });
+    driver_state_sub_ = test_node_->create_subscription<RobotDriverStateMsg>(
+      panther_hardware_interfaces_test::kRobotDriverStateTopic, rclcpp::SensorDataQoS(),
+      [&](const RobotDriverStateMsg::SharedPtr msg) { driver_state_msg_ = msg; });
 
     panther_system_ros_interface_ =
       std::make_unique<PantherSystemRosInterface>("hardware_controller");
@@ -50,8 +51,8 @@ class TestPantherSystemRosInterface : public ::testing::Test
 
 protected:
   rclcpp::Node::SharedPtr test_node_;
-  rclcpp::Subscription<panther_msgs::msg::DriverState>::SharedPtr driver_state_sub_;
-  panther_msgs::msg::DriverState::SharedPtr driver_state_msg_;
+  rclcpp::Subscription<RobotDriverStateMsg>::SharedPtr driver_state_sub_;
+  RobotDriverStateMsg::SharedPtr driver_state_msg_;
   std::unique_ptr<panther_hardware_interfaces::PantherSystemRosInterface>
     panther_system_ros_interface_;
 };
@@ -95,7 +96,7 @@ TEST(TestPantherSystemRosInterfaceInitialization, NodeCreation)
 TEST(TestPantherSystemRosInterfaceInitialization, Activation)
 {
   using panther_hardware_interfaces::PantherSystemRosInterface;
-  using panther_hardware_interfaces_test::kMotorControllersStateTopic;
+  using panther_hardware_interfaces_test::kRobotDriverStateTopic;
 
   std::map<std::string, std::vector<std::string>> service_names_and_types;
   std::map<std::string, std::vector<std::string>> topic_names_and_types;
@@ -109,24 +110,21 @@ TEST(TestPantherSystemRosInterfaceInitialization, Activation)
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
   topic_names_and_types = test_node->get_topic_names_and_types();
-  ASSERT_TRUE(
-    topic_names_and_types.find(kMotorControllersStateTopic) != topic_names_and_types.end());
+  ASSERT_TRUE(topic_names_and_types.find(kRobotDriverStateTopic) != topic_names_and_types.end());
 
   panther_system_ros_interface.reset();
 
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
   topic_names_and_types = test_node->get_topic_names_and_types();
-  ASSERT_FALSE(
-    topic_names_and_types.find(kMotorControllersStateTopic) != topic_names_and_types.end());
+  ASSERT_FALSE(topic_names_and_types.find(kRobotDriverStateTopic) != topic_names_and_types.end());
 
   panther_system_ros_interface = std::make_unique<PantherSystemRosInterface>("hardware_controller");
 
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
   topic_names_and_types = test_node->get_topic_names_and_types();
-  ASSERT_TRUE(
-    topic_names_and_types.find(kMotorControllersStateTopic) != topic_names_and_types.end());
+  ASSERT_TRUE(topic_names_and_types.find(kRobotDriverStateTopic) != topic_names_and_types.end());
 
   panther_system_ros_interface.reset();
 }
@@ -154,22 +152,22 @@ TEST_F(TestPantherSystemRosInterface, ErrorFlags)
   rear.SetDriverState(rear_driver_state, false);
 
   panther_system_ros_interface_->UpdateMsgErrorFlags(front, rear);
-  panther_system_ros_interface_->PublishDriverState();
+  panther_system_ros_interface_->PublishRobotDriverState();
 
   ASSERT_TRUE(
     panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5)));
 
-  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(0).state.fault_flag.overheat);
-  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(0).state.script_flag.encoder_disconnected);
-  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(0).state.left_motor_runtime_error.loop_error);
+  EXPECT_TRUE(driver_state_msg_->drivers_states.at(0).state.fault_flag.overheat);
+  EXPECT_TRUE(driver_state_msg_->drivers_states.at(0).state.script_flag.encoder_disconnected);
+  EXPECT_TRUE(driver_state_msg_->drivers_states.at(0).state.left_motor_runtime_error.loop_error);
   EXPECT_TRUE(
-    driver_state_msg_->motor_controllers.at(0).state.right_motor_runtime_error.safety_stop_active);
+    driver_state_msg_->drivers_states.at(0).state.right_motor_runtime_error.safety_stop_active);
 
-  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(1).state.fault_flag.overvoltage);
-  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(1).state.script_flag.loop_error);
-  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(1)
-                .state.left_motor_runtime_error.forward_limit_triggered);
-  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(1)
+  EXPECT_TRUE(driver_state_msg_->drivers_states.at(1).state.fault_flag.overvoltage);
+  EXPECT_TRUE(driver_state_msg_->drivers_states.at(1).state.script_flag.loop_error);
+  EXPECT_TRUE(
+    driver_state_msg_->drivers_states.at(1).state.left_motor_runtime_error.forward_limit_triggered);
+  EXPECT_TRUE(driver_state_msg_->drivers_states.at(1)
                 .state.right_motor_runtime_error.reverse_limit_triggered);
 }
 
@@ -202,39 +200,35 @@ TEST_F(TestPantherSystemRosInterface, DriversStates)
   rear.SetBatteryCurrent2(r_battery_current_2);
 
   panther_system_ros_interface_->UpdateMsgDriversStates(front, rear);
-  panther_system_ros_interface_->PublishDriverState();
+  panther_system_ros_interface_->PublishRobotDriverState();
 
   ASSERT_TRUE(
     panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5)));
 
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->motor_controllers.at(0).state.temperature),
-    f_temp);
+    static_cast<std::int16_t>(driver_state_msg_->drivers_states.at(0).state.temperature), f_temp);
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->motor_controllers.at(1).state.temperature),
-    r_temp);
+    static_cast<std::int16_t>(driver_state_msg_->drivers_states.at(1).state.temperature), r_temp);
 
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(
-      driver_state_msg_->motor_controllers.at(0).state.heatsink_temperature),
+    static_cast<std::int16_t>(driver_state_msg_->drivers_states.at(0).state.heatsink_temperature),
     f_heatsink_temp);
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(
-      driver_state_msg_->motor_controllers.at(1).state.heatsink_temperature),
+    static_cast<std::int16_t>(driver_state_msg_->drivers_states.at(1).state.heatsink_temperature),
     r_heatsink_temp);
 
   EXPECT_FLOAT_EQ(
-    static_cast<std::uint16_t>(driver_state_msg_->motor_controllers.at(0).state.voltage * 10.0),
+    static_cast<std::uint16_t>(driver_state_msg_->drivers_states.at(0).state.voltage * 10.0),
     f_volt);
   EXPECT_FLOAT_EQ(
-    static_cast<std::uint16_t>(driver_state_msg_->motor_controllers.at(1).state.voltage * 10.0),
+    static_cast<std::uint16_t>(driver_state_msg_->drivers_states.at(1).state.voltage * 10.0),
     r_volt);
 
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->motor_controllers.at(0).state.current * 10.0),
+    static_cast<std::int16_t>(driver_state_msg_->drivers_states.at(0).state.current * 10.0),
     (f_battery_current_1 + f_battery_current_2));
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->motor_controllers.at(1).state.current * 10.0),
+    static_cast<std::int16_t>(driver_state_msg_->drivers_states.at(1).state.current * 10.0),
     (r_battery_current_1 + r_battery_current_2));
 }
 
@@ -258,7 +252,7 @@ TEST_F(TestPantherSystemRosInterface, Errors)
 
   panther_system_ros_interface_->UpdateMsgErrors(can_errors);
 
-  panther_system_ros_interface_->PublishDriverState();
+  panther_system_ros_interface_->PublishRobotDriverState();
 
   ASSERT_TRUE(
     panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5)));
@@ -269,14 +263,14 @@ TEST_F(TestPantherSystemRosInterface, Errors)
   EXPECT_FALSE(driver_state_msg_->read_pdo_motor_states_error);
   EXPECT_FALSE(driver_state_msg_->read_pdo_driver_state_error);
 
-  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(0).state.motor_states_data_timed_out);
-  EXPECT_FALSE(driver_state_msg_->motor_controllers.at(1).state.motor_states_data_timed_out);
+  EXPECT_TRUE(driver_state_msg_->drivers_states.at(0).state.motor_states_data_timed_out);
+  EXPECT_FALSE(driver_state_msg_->drivers_states.at(1).state.motor_states_data_timed_out);
 
-  EXPECT_FALSE(driver_state_msg_->motor_controllers.at(0).state.driver_state_data_timed_out);
-  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(1).state.driver_state_data_timed_out);
+  EXPECT_FALSE(driver_state_msg_->drivers_states.at(0).state.driver_state_data_timed_out);
+  EXPECT_TRUE(driver_state_msg_->drivers_states.at(1).state.driver_state_data_timed_out);
 
-  EXPECT_FALSE(driver_state_msg_->motor_controllers.at(0).state.can_error);
-  EXPECT_TRUE(driver_state_msg_->motor_controllers.at(1).state.can_error);
+  EXPECT_FALSE(driver_state_msg_->drivers_states.at(0).state.can_error);
+  EXPECT_TRUE(driver_state_msg_->drivers_states.at(1).state.can_error);
 }
 
 int main(int argc, char ** argv)
diff --git a/panther_hardware_interfaces/test/utils/test_constants.hpp b/panther_hardware_interfaces/test/utils/test_constants.hpp
index 92e4fe83..be0d201e 100644
--- a/panther_hardware_interfaces/test/utils/test_constants.hpp
+++ b/panther_hardware_interfaces/test/utils/test_constants.hpp
@@ -92,7 +92,7 @@ const std::map<std::string, std::string> kDefaultParamMap = {
 const std::vector<std::string> kDefaultJoints = {
   "fl_wheel_joint", "fr_wheel_joint", "rl_wheel_joint", "rr_wheel_joint"};
 
-const std::string kMotorControllersStateTopic = "/hardware_controller/motor_controllers_state";
+const std::string kRobotDriverStateTopic = "/hardware_controller/robot_driver_state";
 
 }  // namespace panther_hardware_interfaces_test
 
diff --git a/panther_manager/README.md b/panther_manager/README.md
index b4ea0936..a717801d 100644
--- a/panther_manager/README.md
+++ b/panther_manager/README.md
@@ -58,7 +58,7 @@ Node responsible for managing safety features, and software shutdown of componen
 - `battery/battery_status` [*sensor_msgs/BatteryState*]: State of the internal Battery.
 - `hardware/e_stop` [*std_msgs/Bool*]: State of emergency stop.
 - `hardware/io_state` [*panther_msgs/IOState*]: State of IO pins.
-- `hardware/motor_controllers_state` [*panther_msgs/DriverState*]: State of motor controllers.
+- `hardware/robot_driver_state` [*panther_msgs/DriverState*]: State of motor controllers.
 - `system_status` [*panther_msgs/SystemStatus*]: Built-in computer system status, includes the most important computation-related parameters.
 
 #### Service Clients (for Default Trees)
diff --git a/panther_manager/include/panther_manager/safety_manager_node.hpp b/panther_manager/include/panther_manager/safety_manager_node.hpp
index 59b97f1f..cde19e96 100644
--- a/panther_manager/include/panther_manager/safety_manager_node.hpp
+++ b/panther_manager/include/panther_manager/safety_manager_node.hpp
@@ -25,8 +25,8 @@
 #include "sensor_msgs/msg/battery_state.hpp"
 #include "std_msgs/msg/bool.hpp"
 
-#include "panther_msgs/msg/driver_state.hpp"
 #include "panther_msgs/msg/io_state.hpp"
+#include "panther_msgs/msg/robot_driver_state.hpp"
 #include "panther_msgs/msg/system_status.hpp"
 
 #include "panther_utils/moving_average.hpp"
@@ -38,7 +38,7 @@ namespace panther_manager
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
 using BoolMsg = std_msgs::msg::Bool;
-using DriverStateMsg = panther_msgs::msg::DriverState;
+using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
 using IOStateMsg = panther_msgs::msg::IOState;
 using SystemStatusMsg = panther_msgs::msg::SystemStatus;
 
@@ -75,7 +75,7 @@ class SafetyManagerNode : public rclcpp::Node
 
 private:
   void BatteryCB(const BatteryStateMsg::SharedPtr battery);
-  void DriverStateCB(const DriverStateMsg::SharedPtr driver_state);
+  void RobotDriverStateCB(const RobotDriverStateMsg::SharedPtr driver_state);
   void EStopCB(const BoolMsg::SharedPtr e_stop);
   void IOStateCB(const IOStateMsg::SharedPtr io_state);
   void SystemStatusCB(const SystemStatusMsg::SharedPtr system_status);
@@ -90,7 +90,7 @@ class SafetyManagerNode : public rclcpp::Node
   float update_charging_anim_step_;
 
   rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_sub_;
-  rclcpp::Subscription<DriverStateMsg>::SharedPtr driver_state_sub_;
+  rclcpp::Subscription<RobotDriverStateMsg>::SharedPtr driver_state_sub_;
   rclcpp::Subscription<BoolMsg>::SharedPtr e_stop_sub_;
   rclcpp::Subscription<IOStateMsg>::SharedPtr io_state_sub_;
   rclcpp::Subscription<SystemStatusMsg>::SharedPtr system_status_sub_;
diff --git a/panther_manager/src/safety_manager_node.cpp b/panther_manager/src/safety_manager_node.cpp
index 6933968d..6378c77e 100644
--- a/panther_manager/src/safety_manager_node.cpp
+++ b/panther_manager/src/safety_manager_node.cpp
@@ -81,8 +81,9 @@ void SafetyManagerNode::Initialize()
 
   battery_sub_ = this->create_subscription<BatteryStateMsg>(
     "battery/battery_status", 10, std::bind(&SafetyManagerNode::BatteryCB, this, _1));
-  driver_state_sub_ = this->create_subscription<DriverStateMsg>(
-    "hardware/motor_controllers_state", 10, std::bind(&SafetyManagerNode::DriverStateCB, this, _1));
+  driver_state_sub_ = this->create_subscription<RobotDriverStateMsg>(
+    "hardware/robot_driver_state", 10,
+    std::bind(&SafetyManagerNode::RobotDriverStateCB, this, _1));
   e_stop_sub_ = this->create_subscription<BoolMsg>(
     "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&SafetyManagerNode::EStopCB, this, _1));
@@ -210,14 +211,14 @@ void SafetyManagerNode::BatteryCB(const BatteryStateMsg::SharedPtr battery)
   safety_tree_manager_->GetBlackboard()->set<double>("bat_temp", battery_temp_ma_->GetAverage());
 }
 
-void SafetyManagerNode::DriverStateCB(const DriverStateMsg::SharedPtr driver_state)
+void SafetyManagerNode::RobotDriverStateCB(const RobotDriverStateMsg::SharedPtr driver_state)
 {
-  if (driver_state->motor_controllers.empty()) {
+  if (driver_state->drivers_states.empty()) {
     RCLCPP_WARN(this->get_logger(), "Received empty driver state message.");
     return;
   }
 
-  for (auto & driver : driver_state->motor_controllers) {
+  for (auto & driver : driver_state->drivers_states) {
     if (driver_temp_ma_.find(driver.name) == driver_temp_ma_.end()) {
       RCLCPP_DEBUG(
         this->get_logger(), "Creating moving average for driver '%s'", driver.name.c_str());
diff --git a/panther_manager/test/test_safety_manager_node.cpp b/panther_manager/test/test_safety_manager_node.cpp
index ecd497dd..8ebf2dc2 100644
--- a/panther_manager/test/test_safety_manager_node.cpp
+++ b/panther_manager/test/test_safety_manager_node.cpp
@@ -194,14 +194,14 @@ TEST_F(TestSafetyManagerNode, DriverStateCBBlackboardUpdate)
 {
   const float expected_temp = 21.0;
 
-  panther_msgs::msg::MotorController motor_controller;
+  panther_msgs::msg::DriverStateNamed motor_controller;
   motor_controller.state.temperature = expected_temp;
 
-  auto driver_state_msg = panther_msgs::msg::DriverState();
-  driver_state_msg.motor_controllers.push_back(motor_controller);
+  auto driver_state_msg = panther_msgs::msg::RobotDriverState();
+  driver_state_msg.drivers_states.push_back(motor_controller);
 
   panther_utils::test_utils::PublishAndSpin(
-    safety_manager_node_, "hardware/motor_controllers_state", driver_state_msg);
+    safety_manager_node_, "hardware/robot_driver_state", driver_state_msg);
 
   auto blackboard = safety_manager_node_->GetSafetyTreeBlackboard();
   EXPECT_FLOAT_EQ(blackboard->get<float>("driver_temp"), expected_temp);

From baca331c4e1151fa24fde7e770b1ce91e4a9a901 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Thu, 22 Aug 2024 12:03:00 +0000
Subject: [PATCH 010/100] update

---
 .../src/battery/roboteq_battery.cpp           |  6 +--
 .../test/battery/test_roboteq_battery.cpp     | 18 ++++----
 .../test/test_battery_driver_node_roboteq.cpp |  8 ++--
 .../src/panther_system/panther_system.cpp     |  4 +-
 .../panther_system_ros_interface.cpp          | 18 ++++----
 .../panther_system/test_panther_system.cpp    | 18 ++++----
 .../test_panther_system_ros_interface.cpp     | 46 +++++++++----------
 panther_manager/src/safety_manager_node.cpp   |  4 +-
 .../test/test_safety_manager_node.cpp         |  2 +-
 9 files changed, 62 insertions(+), 62 deletions(-)

diff --git a/panther_battery/src/battery/roboteq_battery.cpp b/panther_battery/src/battery/roboteq_battery.cpp
index 0c441781..6cd0f3fd 100644
--- a/panther_battery/src/battery/roboteq_battery.cpp
+++ b/panther_battery/src/battery/roboteq_battery.cpp
@@ -49,13 +49,13 @@ void RoboteqBattery::Update(const rclcpp::Time & header_stamp, const bool /* cha
   float voltage = 0.0f;
   float current = 0.0f;
   std::for_each(
-    driver_state_->drivers_states.begin(), driver_state_->drivers_states.end(),
+    driver_state_->driver_states.begin(), driver_state_->driver_states.end(),
     [&voltage, &current](const DriverStateNamedMsg & driver) {
       voltage += driver.state.voltage;
       current += driver.state.current;
     });
 
-  voltage_raw_ = voltage / driver_state_->drivers_states.size();
+  voltage_raw_ = voltage / driver_state_->driver_states.size();
   current_raw_ = current;
   voltage_ma_->Roll(voltage_raw_);
   current_ma_->Roll(current_raw_);
@@ -153,7 +153,7 @@ std::uint8_t RoboteqBattery::GetBatteryHealth(const float voltage)
 bool RoboteqBattery::DriverStateHeartbeatTimeout()
 {
   return std::any_of(
-    driver_state_->drivers_states.begin(), driver_state_->drivers_states.end(),
+    driver_state_->driver_states.begin(), driver_state_->driver_states.end(),
     [](const DriverStateNamedMsg & driver) { return driver.state.heartbeat_timeout; });
 }
 
diff --git a/panther_battery/test/battery/test_roboteq_battery.cpp b/panther_battery/test/battery/test_roboteq_battery.cpp
index c8da21e2..8ca850f1 100644
--- a/panther_battery/test/battery/test_roboteq_battery.cpp
+++ b/panther_battery/test/battery/test_roboteq_battery.cpp
@@ -81,17 +81,17 @@ void TestRoboteqBattery::UpdateBattery(const float voltage, const float current)
 {
   if (!driver_state_) {
     driver_state_ = std::make_shared<RobotDriverStateMsg>();
-    driver_state_->drivers_states.push_back(panther_msgs::msg::DriverStateNamed());
-    driver_state_->drivers_states.push_back(panther_msgs::msg::DriverStateNamed());
+    driver_state_->driver_states.push_back(panther_msgs::msg::DriverStateNamed());
+    driver_state_->driver_states.push_back(panther_msgs::msg::DriverStateNamed());
   }
 
   auto stamp = rclcpp::Time(0, 0, RCL_ROS_TIME);
 
   driver_state_->header.stamp = stamp;
-  driver_state_->drivers_states.at(0).state.voltage = voltage;
-  driver_state_->drivers_states.at(1).state.voltage = voltage;
-  driver_state_->drivers_states.at(0).state.current = current;
-  driver_state_->drivers_states.at(1).state.current = current;
+  driver_state_->driver_states.at(0).state.voltage = voltage;
+  driver_state_->driver_states.at(1).state.voltage = voltage;
+  driver_state_->driver_states.at(0).state.current = current;
+  driver_state_->driver_states.at(1).state.current = current;
 
   battery_->Update(stamp, false);
   battery_state_ = battery_->GetBatteryMsg();
@@ -243,10 +243,10 @@ TEST_F(TestRoboteqBattery, ValidateRobotDriverStateMsg)
   EXPECT_NO_THROW(battery_->ValidateRobotDriverStateMsg(stamp));
 
   // Check heartbeat timeout error throw
-  driver_state_->drivers_states.at(0).state.heartbeat_timeout = true;
+  driver_state_->driver_states.at(0).state.heartbeat_timeout = true;
   EXPECT_THROW(battery_->ValidateRobotDriverStateMsg(stamp), std::runtime_error);
-  driver_state_->drivers_states.at(0).state.heartbeat_timeout = false;
-  driver_state_->drivers_states.at(1).state.heartbeat_timeout = true;
+  driver_state_->driver_states.at(0).state.heartbeat_timeout = false;
+  driver_state_->driver_states.at(1).state.heartbeat_timeout = true;
   EXPECT_THROW(battery_->ValidateRobotDriverStateMsg(stamp), std::runtime_error);
 }
 
diff --git a/panther_battery/test/test_battery_driver_node_roboteq.cpp b/panther_battery/test/test_battery_driver_node_roboteq.cpp
index ae1fa1db..611b5d64 100644
--- a/panther_battery/test/test_battery_driver_node_roboteq.cpp
+++ b/panther_battery/test/test_battery_driver_node_roboteq.cpp
@@ -50,8 +50,8 @@ TEST_F(TestBatteryNodeRoboteq, BatteryValues)
 
   RobotDriverStateMsg driver_state;
   driver_state.header.stamp = battery_driver_node_->get_clock()->now();
-  driver_state.drivers_states.push_back(motor_controller);
-  driver_state.drivers_states.push_back(motor_controller);
+  driver_state.driver_states.push_back(motor_controller);
+  driver_state.driver_states.push_back(motor_controller);
   driver_state_pub_->publish(driver_state);
 
   ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
@@ -90,8 +90,8 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout)
 
   RobotDriverStateMsg driver_state;
   driver_state.header.stamp = battery_driver_node_->get_clock()->now();
-  driver_state.drivers_states.push_back(motor_controller);
-  driver_state.drivers_states.push_back(motor_controller);
+  driver_state.driver_states.push_back(motor_controller);
+  driver_state.driver_states.push_back(motor_controller);
   driver_state_pub_->publish(driver_state);
 
   ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
diff --git a/panther_hardware_interfaces/src/panther_system/panther_system.cpp b/panther_hardware_interfaces/src/panther_system/panther_system.cpp
index 4134d9c3..74d63950 100644
--- a/panther_hardware_interfaces/src/panther_system/panther_system.cpp
+++ b/panther_hardware_interfaces/src/panther_system/panther_system.cpp
@@ -729,11 +729,11 @@ void PantherSystem::DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper &
   const auto front_driver_state = motors_controller_->GetFrontData().GetDriverState();
   const auto rear_driver_state = motors_controller_->GetRearData().GetDriverState();
 
-  auto drivers_states_with_names = {
+  auto driver_states_with_names = {
     std::make_pair(std::string("Front"), front_driver_state),
     std::make_pair(std::string("Rear"), rear_driver_state)};
 
-  for (const auto & [driver_name, driver_state] : drivers_states_with_names) {
+  for (const auto & [driver_name, driver_state] : driver_states_with_names) {
     status.add(driver_name + " driver voltage (V)", driver_state.GetVoltage());
     status.add(driver_name + " driver current (A)", driver_state.GetCurrent());
     status.add(driver_name + " driver temperature (\u00B0C)", driver_state.GetTemperature());
diff --git a/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp b/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp
index 22a02752..e6650784 100644
--- a/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp
+++ b/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp
@@ -139,13 +139,13 @@ void PantherSystemRosInterface::UpdateMsgErrorFlags(
 
   front_driver_state.state.fault_flag = front.GetFaultFlag().GetMessage();
   front_driver_state.state.script_flag = front.GetScriptFlag().GetMessage();
-  front_driver_state.state.left_motor_runtime_error = front.GetLeftRuntimeError().GetMessage();
-  front_driver_state.state.right_motor_runtime_error = front.GetRightRuntimeError().GetMessage();
+  front_driver_state.state.channel_2_motor_runtime_error = front.GetLeftRuntimeError().GetMessage();
+  front_driver_state.state.channel_1_motor_runtime_error = front.GetRightRuntimeError().GetMessage();
 
   rear_driver_state.state.fault_flag = rear.GetFaultFlag().GetMessage();
   rear_driver_state.state.script_flag = rear.GetScriptFlag().GetMessage();
-  rear_driver_state.state.left_motor_runtime_error = rear.GetLeftRuntimeError().GetMessage();
-  rear_driver_state.state.right_motor_runtime_error = rear.GetRightRuntimeError().GetMessage();
+  rear_driver_state.state.channel_2_motor_runtime_error = rear.GetLeftRuntimeError().GetMessage();
+  rear_driver_state.state.channel_1_motor_runtime_error = rear.GetRightRuntimeError().GetMessage();
 }
 
 void PantherSystemRosInterface::UpdateMsgDriversStates(
@@ -309,20 +309,20 @@ void PantherSystemRosInterface::InitializeRobotDriverStateMsg()
   rear_driver_state.name = DriverStateNamedMsg::NAME_REAR;
 
   auto & driver_state = realtime_driver_state_publisher_->msg_;
-  driver_state.drivers_states.push_back(front_driver_state);
-  driver_state.drivers_states.push_back(rear_driver_state);
+  driver_state.driver_states.push_back(front_driver_state);
+  driver_state.driver_states.push_back(rear_driver_state);
 }
 
 DriverStateNamedMsg & PantherSystemRosInterface::GetDriverStateByName(
   RobotDriverStateMsg & robot_driver_state, const std::string & name)
 {
-  auto & drivers_states = robot_driver_state.drivers_states;
+  auto & driver_states = robot_driver_state.driver_states;
 
   auto it = std::find_if(
-    drivers_states.begin(), drivers_states.end(),
+    driver_states.begin(), driver_states.end(),
     [&name](const DriverStateNamedMsg & msg) { return msg.name == name; });
 
-  if (it == drivers_states.end()) {
+  if (it == driver_states.end()) {
     throw std::runtime_error("Driver with name '" + name + "' not found.");
   }
 
diff --git a/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp b/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp
index 635715fb..3b8c0829 100644
--- a/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp
+++ b/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp
@@ -453,26 +453,26 @@ TEST_F(TestPantherSystem, ReadOtherRoboteqParamsPantherSystem)
 
   ASSERT_TRUE(state_msg);
 
-  ASSERT_EQ(static_cast<std::int16_t>(state_msg->drivers_states.at(0).state.temperature), f_temp);
-  ASSERT_EQ(static_cast<std::int16_t>(state_msg->drivers_states.at(1).state.temperature), r_temp);
+  ASSERT_EQ(static_cast<std::int16_t>(state_msg->driver_states.at(0).state.temperature), f_temp);
+  ASSERT_EQ(static_cast<std::int16_t>(state_msg->driver_states.at(1).state.temperature), r_temp);
 
   ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->drivers_states.at(0).state.heatsink_temperature),
+    static_cast<std::int16_t>(state_msg->driver_states.at(0).state.heatsink_temperature),
     f_heatsink_temp);
   ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->drivers_states.at(1).state.heatsink_temperature),
+    static_cast<std::int16_t>(state_msg->driver_states.at(1).state.heatsink_temperature),
     r_heatsink_temp);
 
   ASSERT_EQ(
-    static_cast<std::uint16_t>(state_msg->drivers_states.at(0).state.voltage * 10.0), f_volt);
+    static_cast<std::uint16_t>(state_msg->driver_states.at(0).state.voltage * 10.0), f_volt);
   ASSERT_EQ(
-    static_cast<std::uint16_t>(state_msg->drivers_states.at(1).state.voltage * 10.0), r_volt);
+    static_cast<std::uint16_t>(state_msg->driver_states.at(1).state.voltage * 10.0), r_volt);
 
   ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->drivers_states.at(0).state.current * 10.0),
+    static_cast<std::int16_t>(state_msg->driver_states.at(0).state.current * 10.0),
     (f_battery_current_1 + f_battery_current_2));
   ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->drivers_states.at(1).state.current * 10.0),
+    static_cast<std::int16_t>(state_msg->driver_states.at(1).state.current * 10.0),
     (r_battery_current_1 + r_battery_current_2));
 
   pth_test_.ShutdownPantherSystem();
@@ -505,7 +505,7 @@ TEST_F(TestPantherSystem, EncoderDisconnectedPantherSystem)
   pth_test_.GetResourceManager()->read(TIME, PERIOD);
 
   ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5)));
-  ASSERT_TRUE(state_msg->drivers_states.at(0).state.script_flag.encoder_disconnected);
+  ASSERT_TRUE(state_msg->driver_states.at(0).state.script_flag.encoder_disconnected);
 
   // writing should be blocked - error
 
diff --git a/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp b/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
index 9412242a..bf522dce 100644
--- a/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
+++ b/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
@@ -157,18 +157,18 @@ TEST_F(TestPantherSystemRosInterface, ErrorFlags)
   ASSERT_TRUE(
     panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5)));
 
-  EXPECT_TRUE(driver_state_msg_->drivers_states.at(0).state.fault_flag.overheat);
-  EXPECT_TRUE(driver_state_msg_->drivers_states.at(0).state.script_flag.encoder_disconnected);
-  EXPECT_TRUE(driver_state_msg_->drivers_states.at(0).state.left_motor_runtime_error.loop_error);
+  EXPECT_TRUE(driver_state_msg_->driver_states.at(0).state.fault_flag.overheat);
+  EXPECT_TRUE(driver_state_msg_->driver_states.at(0).state.script_flag.encoder_disconnected);
+  EXPECT_TRUE(driver_state_msg_->driver_states.at(0).state.channel_2_motor_runtime_error.loop_error);
   EXPECT_TRUE(
-    driver_state_msg_->drivers_states.at(0).state.right_motor_runtime_error.safety_stop_active);
+    driver_state_msg_->driver_states.at(0).state.channel_1_motor_runtime_error.safety_stop_active);
 
-  EXPECT_TRUE(driver_state_msg_->drivers_states.at(1).state.fault_flag.overvoltage);
-  EXPECT_TRUE(driver_state_msg_->drivers_states.at(1).state.script_flag.loop_error);
+  EXPECT_TRUE(driver_state_msg_->driver_states.at(1).state.fault_flag.overvoltage);
+  EXPECT_TRUE(driver_state_msg_->driver_states.at(1).state.script_flag.loop_error);
   EXPECT_TRUE(
-    driver_state_msg_->drivers_states.at(1).state.left_motor_runtime_error.forward_limit_triggered);
-  EXPECT_TRUE(driver_state_msg_->drivers_states.at(1)
-                .state.right_motor_runtime_error.reverse_limit_triggered);
+    driver_state_msg_->driver_states.at(1).state.channel_2_motor_runtime_error.forward_limit_triggered);
+  EXPECT_TRUE(driver_state_msg_->driver_states.at(1)
+                .state.channel_1_motor_runtime_error.reverse_limit_triggered);
 }
 
 TEST_F(TestPantherSystemRosInterface, DriversStates)
@@ -206,29 +206,29 @@ TEST_F(TestPantherSystemRosInterface, DriversStates)
     panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5)));
 
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->drivers_states.at(0).state.temperature), f_temp);
+    static_cast<std::int16_t>(driver_state_msg_->driver_states.at(0).state.temperature), f_temp);
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->drivers_states.at(1).state.temperature), r_temp);
+    static_cast<std::int16_t>(driver_state_msg_->driver_states.at(1).state.temperature), r_temp);
 
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->drivers_states.at(0).state.heatsink_temperature),
+    static_cast<std::int16_t>(driver_state_msg_->driver_states.at(0).state.heatsink_temperature),
     f_heatsink_temp);
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->drivers_states.at(1).state.heatsink_temperature),
+    static_cast<std::int16_t>(driver_state_msg_->driver_states.at(1).state.heatsink_temperature),
     r_heatsink_temp);
 
   EXPECT_FLOAT_EQ(
-    static_cast<std::uint16_t>(driver_state_msg_->drivers_states.at(0).state.voltage * 10.0),
+    static_cast<std::uint16_t>(driver_state_msg_->driver_states.at(0).state.voltage * 10.0),
     f_volt);
   EXPECT_FLOAT_EQ(
-    static_cast<std::uint16_t>(driver_state_msg_->drivers_states.at(1).state.voltage * 10.0),
+    static_cast<std::uint16_t>(driver_state_msg_->driver_states.at(1).state.voltage * 10.0),
     r_volt);
 
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->drivers_states.at(0).state.current * 10.0),
+    static_cast<std::int16_t>(driver_state_msg_->driver_states.at(0).state.current * 10.0),
     (f_battery_current_1 + f_battery_current_2));
   EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->drivers_states.at(1).state.current * 10.0),
+    static_cast<std::int16_t>(driver_state_msg_->driver_states.at(1).state.current * 10.0),
     (r_battery_current_1 + r_battery_current_2));
 }
 
@@ -263,14 +263,14 @@ TEST_F(TestPantherSystemRosInterface, Errors)
   EXPECT_FALSE(driver_state_msg_->read_pdo_motor_states_error);
   EXPECT_FALSE(driver_state_msg_->read_pdo_driver_state_error);
 
-  EXPECT_TRUE(driver_state_msg_->drivers_states.at(0).state.motor_states_data_timed_out);
-  EXPECT_FALSE(driver_state_msg_->drivers_states.at(1).state.motor_states_data_timed_out);
+  EXPECT_TRUE(driver_state_msg_->driver_states.at(0).state.motor_states_data_timed_out);
+  EXPECT_FALSE(driver_state_msg_->driver_states.at(1).state.motor_states_data_timed_out);
 
-  EXPECT_FALSE(driver_state_msg_->drivers_states.at(0).state.driver_state_data_timed_out);
-  EXPECT_TRUE(driver_state_msg_->drivers_states.at(1).state.driver_state_data_timed_out);
+  EXPECT_FALSE(driver_state_msg_->driver_states.at(0).state.driver_state_data_timed_out);
+  EXPECT_TRUE(driver_state_msg_->driver_states.at(1).state.driver_state_data_timed_out);
 
-  EXPECT_FALSE(driver_state_msg_->drivers_states.at(0).state.can_error);
-  EXPECT_TRUE(driver_state_msg_->drivers_states.at(1).state.can_error);
+  EXPECT_FALSE(driver_state_msg_->driver_states.at(0).state.can_error);
+  EXPECT_TRUE(driver_state_msg_->driver_states.at(1).state.can_error);
 }
 
 int main(int argc, char ** argv)
diff --git a/panther_manager/src/safety_manager_node.cpp b/panther_manager/src/safety_manager_node.cpp
index 6378c77e..51c2dced 100644
--- a/panther_manager/src/safety_manager_node.cpp
+++ b/panther_manager/src/safety_manager_node.cpp
@@ -213,12 +213,12 @@ void SafetyManagerNode::BatteryCB(const BatteryStateMsg::SharedPtr battery)
 
 void SafetyManagerNode::RobotDriverStateCB(const RobotDriverStateMsg::SharedPtr driver_state)
 {
-  if (driver_state->drivers_states.empty()) {
+  if (driver_state->driver_states.empty()) {
     RCLCPP_WARN(this->get_logger(), "Received empty driver state message.");
     return;
   }
 
-  for (auto & driver : driver_state->drivers_states) {
+  for (auto & driver : driver_state->driver_states) {
     if (driver_temp_ma_.find(driver.name) == driver_temp_ma_.end()) {
       RCLCPP_DEBUG(
         this->get_logger(), "Creating moving average for driver '%s'", driver.name.c_str());
diff --git a/panther_manager/test/test_safety_manager_node.cpp b/panther_manager/test/test_safety_manager_node.cpp
index 8ebf2dc2..48b8d91b 100644
--- a/panther_manager/test/test_safety_manager_node.cpp
+++ b/panther_manager/test/test_safety_manager_node.cpp
@@ -198,7 +198,7 @@ TEST_F(TestSafetyManagerNode, DriverStateCBBlackboardUpdate)
   motor_controller.state.temperature = expected_temp;
 
   auto driver_state_msg = panther_msgs::msg::RobotDriverState();
-  driver_state_msg.drivers_states.push_back(motor_controller);
+  driver_state_msg.driver_states.push_back(motor_controller);
 
   panther_utils::test_utils::PublishAndSpin(
     safety_manager_node_, "hardware/robot_driver_state", driver_state_msg);

From 28f06114ba2996ac9bfdb621d748ea805937f31d Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Thu, 22 Aug 2024 12:13:45 +0000
Subject: [PATCH 011/100] review fixes

---
 panther_battery/README.md                         | 2 +-
 panther_hardware_interfaces/README.md             | 2 +-
 panther_manager/README.md                         | 2 +-
 panther_manager/test/test_safety_manager_node.cpp | 6 +++---
 4 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/panther_battery/README.md b/panther_battery/README.md
index 82ffafbe..d33e5ce9 100644
--- a/panther_battery/README.md
+++ b/panther_battery/README.md
@@ -25,7 +25,7 @@ Publishes battery state read from ADC unit for Panther version 1.2 and above, or
 #### Subscribes
 
 - `hardware/io_state` [*panther_msgs/IOState*]: Current state of IO.
-- `hardware/robot_driver_state` [*panther_msgs/DriverState*]: Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.
+- `hardware/robot_driver_state` [*panther_msgs/RobotDriverState*]: Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.
 
 #### Parameters
 
diff --git a/panther_hardware_interfaces/README.md b/panther_hardware_interfaces/README.md
index d2c45648..bd1b7bf3 100644
--- a/panther_hardware_interfaces/README.md
+++ b/panther_hardware_interfaces/README.md
@@ -15,7 +15,7 @@ Plugin responsible for communicating with engine controllers via the CAN bus, pr
 - `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Panther system diagnostic messages.
 - `hardware/e_stop` [*std_msgs/Bool*]: Current E-stop state.
 - `hardware/io_state` [*panther_msgs/IOState*]: Current IO state.
-- `hardware/robot_driver_state` [*panther_msgs/DriverState*]: Current motor controllers' state and error flags.
+- `hardware/robot_driver_state` [*panther_msgs/RobotDriverState*]: Current motor controllers' state and error flags.
 
 #### Service Servers
 
diff --git a/panther_manager/README.md b/panther_manager/README.md
index a717801d..f58327f0 100644
--- a/panther_manager/README.md
+++ b/panther_manager/README.md
@@ -58,7 +58,7 @@ Node responsible for managing safety features, and software shutdown of componen
 - `battery/battery_status` [*sensor_msgs/BatteryState*]: State of the internal Battery.
 - `hardware/e_stop` [*std_msgs/Bool*]: State of emergency stop.
 - `hardware/io_state` [*panther_msgs/IOState*]: State of IO pins.
-- `hardware/robot_driver_state` [*panther_msgs/DriverState*]: State of motor controllers.
+- `hardware/robot_driver_state` [*panther_msgs/RobotDriverState*]: State of motor controllers.
 - `system_status` [*panther_msgs/SystemStatus*]: Built-in computer system status, includes the most important computation-related parameters.
 
 #### Service Clients (for Default Trees)
diff --git a/panther_manager/test/test_safety_manager_node.cpp b/panther_manager/test/test_safety_manager_node.cpp
index 48b8d91b..afd85de9 100644
--- a/panther_manager/test/test_safety_manager_node.cpp
+++ b/panther_manager/test/test_safety_manager_node.cpp
@@ -194,11 +194,11 @@ TEST_F(TestSafetyManagerNode, DriverStateCBBlackboardUpdate)
 {
   const float expected_temp = 21.0;
 
-  panther_msgs::msg::DriverStateNamed motor_controller;
-  motor_controller.state.temperature = expected_temp;
+  panther_msgs::msg::DriverStateNamed driver_state;
+  driver_state.state.temperature = expected_temp;
 
   auto driver_state_msg = panther_msgs::msg::RobotDriverState();
-  driver_state_msg.driver_states.push_back(motor_controller);
+  driver_state_msg.driver_states.push_back(driver_state);
 
   panther_utils::test_utils::PublishAndSpin(
     safety_manager_node_, "hardware/robot_driver_state", driver_state_msg);

From 61ea75d9406c8682d4d6b3abe0774f123359a778 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Fri, 23 Aug 2024 06:11:30 +0000
Subject: [PATCH 012/100] update roboteq driver

---
 .../motors_controller/driver.hpp              | 150 ++++++++++++
 .../roboteq_data_converters.hpp               |  16 +-
 .../motors_controller/roboteq_driver.hpp      | 219 ++++++------------
 .../roboteq_data_converters.cpp               |   4 +-
 .../motors_controller/roboteq_driver.cpp      | 143 ++++++------
 .../motors_controller/test_roboteq_driver.cpp | 109 +++++----
 .../test/utils/fake_can_socket.hpp            |   2 +-
 7 files changed, 363 insertions(+), 280 deletions(-)
 create mode 100644 panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/driver.hpp

diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/driver.hpp
new file mode 100644
index 00000000..87c341d0
--- /dev/null
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/driver.hpp
@@ -0,0 +1,150 @@
+// 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_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_DRIVER_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_DRIVER_HPP_
+
+#include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <future>
+#include <memory>
+#include <mutex>
+#include <string>
+
+#include "lely/coapp/loop_driver.hpp"
+
+namespace panther_hardware_interfaces
+{
+
+struct MotorDriverState
+{
+  std::int32_t pos;
+  std::int16_t vel;
+  std::int16_t current;
+
+  timespec pos_timestamp;
+  timespec vel_current_timestamp;
+};
+
+struct DriverState
+{
+  std::uint8_t fault_flags;
+  std::uint8_t script_flags;
+  std::uint8_t runtime_stat_flag_motor_1;
+  std::uint8_t runtime_stat_flag_motor_2;
+
+  std::int16_t battery_current_1;
+  std::int16_t battery_current_2;
+
+  std::uint16_t battery_voltage;
+
+  std::int16_t mcu_temp;
+  std::int16_t heatsink_temp;
+
+  timespec flags_current_timestamp;
+  timespec voltages_temps_timestamp;
+};
+
+/**
+ * @brief Abstract class for motor driver
+ */
+class MotorDriver
+{
+public:
+  /**
+   * @brief Reads motors' state data returned from  (PDO 1 and 2) and saves
+   * last timestamps
+   */
+  virtual MotorDriverState ReadMotorDriverState() = 0;
+
+  /**
+   * @brief Sends commands to the motors
+   *
+   * @param cmd command value in the range [-1000, 1000]
+   *
+   * @exception std::runtime_error if operation fails
+   */
+  virtual void SendCmdVel(const std::int32_t cmd) = 0;
+
+  /**
+   * @brief Sends a safety stop command to the motor
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void TurnOnSafetyStop() = 0;
+};
+
+/**
+ * @brief Abstract class for  driver
+ */
+class Driver
+{
+public:
+  /**
+   * @brief Triggers boot operations
+   *
+   * @exception std::runtime_error if triggering boot fails
+   */
+  virtual std::future<void> Boot() = 0;
+
+  /**
+   * @brief Returns true if CAN error was detected.
+   */
+  virtual bool IsCANError() const = 0;
+
+  /**
+   * @brief Returns true if heartbeat timeout encountered.
+   */
+  virtual bool IsHeartbeatTimeout() const = 0;
+
+  /**
+   * @brief Reads driver state data returned from  (PDO 3 and 4): error flags, battery
+   * voltage, battery currents (for channel 1 and 2, they are not the same as motor currents),
+   * temperatures. Also saves the last timestamps
+   */
+  virtual DriverState ReadDriverState() = 0;
+
+  /**
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void ResetScript() = 0;
+
+  /**
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void TurnOnEStop() = 0;
+
+  /**
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void TurnOffEStop() = 0;
+
+  /**
+   * @brief Adds a motor driver to the driver
+   */
+  virtual void AddMotorDriver(
+    const std::string name, std::shared_ptr<MotorDriver> motor_driver) = 0;
+
+  virtual std::shared_ptr<MotorDriver> GetMotorDriver(const std::string & name) = 0;
+
+  /**
+   * @brief Alias for a shared pointer to a Driver object.
+   */
+  using SharedPtr = std::shared_ptr<Driver>;
+};
+
+}  // namespace panther_hardware_interfaces
+
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp
index 0a167e7c..4a6d6b25 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp
@@ -71,7 +71,7 @@ class MotorState
 public:
   MotorState(const DrivetrainSettings & drivetrain_settings);
 
-  void SetData(const RoboteqMotorState & motor_state) { motor_state_ = motor_state; };
+  void SetData(const MotorDriverState & motor_state) { motor_state_ = motor_state; };
 
   float GetPosition() const { return motor_state_.pos * roboteq_pos_feedback_to_radians_; }
   float GetVelocity() const
@@ -88,7 +88,7 @@ class MotorState
   float roboteq_vel_feedback_to_radians_per_second_;
   float roboteq_current_feedback_to_newton_meters_;
 
-  RoboteqMotorState motor_state_ = {0, 0, 0};
+  MotorDriverState motor_state_ = {0, 0, 0, {0, 0}, {0, 0}};
 };
 
 /**
@@ -148,10 +148,10 @@ class RuntimeError : public FlagError
  * @brief Class for storing and converting the current state of the Roboteq drivers (temperature,
  * voltage and battery current)
  */
-class DriverState
+class RoboteqDriverState
 {
 public:
-  DriverState() {}
+  RoboteqDriverState() {}
 
   void SetTemperature(const std::int16_t temp) { temp_ = temp; };
   void SetHeatsinkTemperature(const std::int16_t heatsink_temp) { heatsink_temp_ = heatsink_temp; };
@@ -190,9 +190,9 @@ class RoboteqData
   }
 
   void SetMotorsStates(
-    const RoboteqMotorState & left_state, const RoboteqMotorState & right_state,
+    const MotorDriverState & left_state, const MotorDriverState & right_state,
     const bool data_timed_out);
-  void SetDriverState(const RoboteqDriverState & state, const bool data_timed_out);
+  void SetDriverState(const DriverState & state, const bool data_timed_out);
   void SetCANError(const bool can_error) { can_error_ = can_error; }
   void SetHeartbeatTimeout(const bool heartbeat_timeout) { heartbeat_timeout_ = heartbeat_timeout; }
 
@@ -210,7 +210,7 @@ class RoboteqData
 
   const MotorState & GetLeftMotorState() const { return left_motor_state_; }
   const MotorState & GetRightMotorState() const { return right_motor_state_; }
-  const DriverState & GetDriverState() const { return driver_state_; }
+  const RoboteqDriverState & GetDriverState() const { return driver_state_; }
 
   bool IsMotorStatesDataTimedOut() const { return motor_states_data_timed_out_; }
   bool IsDriverStateDataTimedOut() const { return driver_state_data_timed_out_; }
@@ -231,7 +231,7 @@ class RoboteqData
   MotorState left_motor_state_;
   MotorState right_motor_state_;
 
-  DriverState driver_state_;
+  RoboteqDriverState driver_state_;
 
   FaultFlag fault_flags_;
   ScriptFlag script_flags_;
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
index 1fe94746..0bd37821 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
@@ -25,130 +25,19 @@
 
 #include "lely/coapp/loop_driver.hpp"
 
-namespace panther_hardware_interfaces
-{
-
-struct RoboteqMotorState
-{
-  std::int32_t pos;
-  std::int16_t vel;
-  std::int16_t current;
-};
-
-struct RoboteqMotorsStates
-{
-  RoboteqMotorState motor_1;
-  RoboteqMotorState motor_2;
-
-  timespec pos_timestamp;
-  timespec vel_current_timestamp;
-};
+#include "panther_hardware_interfaces/panther_system/motors_controller/driver.hpp"
 
-struct RoboteqDriverState
-{
-  std::uint8_t fault_flags;
-  std::uint8_t script_flags;
-  std::uint8_t runtime_stat_flag_motor_1;
-  std::uint8_t runtime_stat_flag_motor_2;
-
-  std::int16_t battery_current_1;
-  std::int16_t battery_current_2;
-
-  std::uint16_t battery_voltage;
-
-  std::int16_t mcu_temp;
-  std::int16_t heatsink_temp;
-
-  timespec flags_current_timestamp;
-  timespec voltages_temps_timestamp;
-};
-
-/**
- * @brief Abstract class for Roboteq driver
- */
-class RoboteqDriverInterface
+namespace panther_hardware_interfaces
 {
-public:
-  /**
-   * @brief Triggers boot operations
-   *
-   * @exception std::runtime_error if triggering boot fails
-   */
-  virtual std::future<void> Boot() = 0;
-
-  /**
-   * @brief Returns true if CAN error was detected.
-   */
-  virtual bool IsCANError() const = 0;
-
-  /**
-   * @brief Returns true if heartbeat timeout encountered.
-   */
-  virtual bool IsHeartbeatTimeout() const = 0;
-
-  /**
-   * @brief Reads motors' state data returned from Roboteq (PDO 1 and 2) and saves
-   * last timestamps
-   */
-  virtual RoboteqMotorsStates ReadRoboteqMotorsStates() = 0;
-
-  /**
-   * @brief Reads driver state data returned from Roboteq (PDO 3 and 4): error flags, battery
-   * voltage, battery currents (for channel 1 and 2, they are not the same as motor currents),
-   * temperatures. Also saves the last timestamps
-   */
-  virtual RoboteqDriverState ReadRoboteqDriverState() = 0;
-
-  /**
-   * @brief Sends commands to the motors
-   *
-   * @param cmd command value in the range [-1000, 1000]
-   *
-   * @exception std::runtime_error if operation fails
-   */
-  virtual void SendRoboteqCmd(
-    const std::int32_t cmd_channel_1, const std::int32_t cmd_channel_2) = 0;
-
-  /**
-   * @exception std::runtime_error if any operation returns error
-   */
-  virtual void ResetRoboteqScript() = 0;
-
-  /**
-   * @exception std::runtime_error if any operation returns error
-   */
-  virtual void TurnOnEStop() = 0;
-
-  /**
-   * @exception std::runtime_error if any operation returns error
-   */
-  virtual void TurnOffEStop() = 0;
-
-  /**
-   * @brief Sends a safety stop command to the motor connected to channel 1
-   *
-   * @exception std::runtime_error if any operation returns error
-   */
-  virtual void TurnOnSafetyStopChannel1() = 0;
 
-  /**
-   * @brief Sends a safety stop command to the motor connected to channel 2
-   *
-   * @exception std::runtime_error if any operation returns error
-   */
-  virtual void TurnOnSafetyStopChannel2() = 0;
-
-    /**
-   * @brief Alias for a shared pointer to a RoboteqDriverInterface object.
-   */
-  using SharedPtr = std::shared_ptr<RoboteqDriverInterface>;
-};
+// Forward declaration
+class RoboteqMotorDriver;
 
 /**
- * @brief Hardware implementation of RoboteqDriverInterface with lely LoopDriver for Roboteq drivers
+ * @brief Hardware implementation of Driver with lely LoopDriver for Roboteq drivers
  * control
  */
-class RoboteqDriver : public RoboteqDriverInterface, public lely::canopen::LoopDriver
+class RoboteqDriver : public Driver, public lely::canopen::LoopDriver
 {
 public:
   RoboteqDriver(
@@ -172,32 +61,17 @@ class RoboteqDriver : public RoboteqDriverInterface, public lely::canopen::LoopD
    */
   bool IsHeartbeatTimeout() const override { return heartbeat_timeout_.load(); };
 
-  /**
-   * @brief Reads motors' state data returned from Roboteq (PDO 1 and 2) and saves
-   * last timestamps
-   */
-  RoboteqMotorsStates ReadRoboteqMotorsStates() override;
-
   /**
    * @brief Reads driver state data returned from Roboteq (PDO 3 and 4): error flags, battery
    * voltage, battery currents (for channel 1 and 2, they are not the same as motor currents),
    * temperatures. Also saves the last timestamps
    */
-  RoboteqDriverState ReadRoboteqDriverState() override;
-
-  /**
-   * @brief Sends commands to the motors
-   *
-   * @param cmd command value in the range [-1000, 1000]
-   *
-   * @exception std::runtime_error if operation fails
-   */
-  void SendRoboteqCmd(const std::int32_t cmd_channel_1, const std::int32_t cmd_channel_2) override;
+  DriverState ReadDriverState() override;
 
   /**
    * @exception std::runtime_error if any operation returns error
    */
-  void ResetRoboteqScript() override;
+  void ResetScript() override;
 
   /**
    * @exception std::runtime_error if any operation returns error
@@ -210,20 +84,17 @@ class RoboteqDriver : public RoboteqDriverInterface, public lely::canopen::LoopD
   void TurnOffEStop() override;
 
   /**
-   * @brief Sends a safety stop command to the motor connected to channel 1
-   *
-   * @exception std::runtime_error if any operation returns error
+   * @brief Adds a motor driver to the driver
    */
-  void TurnOnSafetyStopChannel1() override;
+  void AddMotorDriver(const std::string name, std::shared_ptr<MotorDriver> motor_driver) override;
 
   /**
-   * @brief Sends a safety stop command to the motor connected to channel 2
+   * @brief Returns a motor driver by name
    *
-   * @exception std::runtime_error if any operation returns error
+   * @exception std::runtime_error if motor driver with the given name does not exist
    */
-  void TurnOnSafetyStopChannel2() override;
+  std::shared_ptr<MotorDriver> GetMotorDriver(const std::string & name) override;
 
-private:
   /**
    * @brief Blocking SDO write operation
    *
@@ -232,6 +103,28 @@ class RoboteqDriver : public RoboteqDriverInterface, public lely::canopen::LoopD
   template <typename T>
   void SyncSDOWrite(const std::uint16_t index, const std::uint8_t subindex, const T data);
 
+  static constexpr std::uint8_t kChannel1 = 1;
+  static constexpr std::uint8_t kChannel2 = 2;
+
+  /**
+   * @brief Returns the last timestamp of the position data for the given channel
+   */
+  timespec GetPositionTimestamp(const std::uint8_t channel)
+  {
+    std::lock_guard<std::mutex> lck(position_timestamp_mtx_);
+    return last_position_timestamps_.at(channel);
+  }
+
+  /**
+   * @brief Returns the last timestamp of the speed and current data for the given channel
+   */
+  timespec GetSpeedCurrentTimestamp(const std::uint8_t channel)
+  {
+    std::lock_guard<std::mutex> lck(speed_current_timestamp_mtx_);
+    return last_speed_current_timestamps_.at(channel);
+  }
+
+private:
   void OnBoot(
     const lely::canopen::NmtState st, const char es, const std::string & what) noexcept override;
   void OnRpdoWrite(const std::uint16_t idx, const std::uint8_t subidx) noexcept override;
@@ -249,10 +142,11 @@ class RoboteqDriver : public RoboteqDriverInterface, public lely::canopen::LoopD
   std::atomic_bool heartbeat_timeout_;
 
   std::mutex position_timestamp_mtx_;
-  timespec last_position_timestamp_;
+  std::map<std::uint8_t, timespec> last_position_timestamps_ = {{kChannel1, {}}, {kChannel2, {}}};
 
   std::mutex speed_current_timestamp_mtx_;
-  timespec last_speed_current_timestamp_;
+  std::map<std::uint8_t, timespec> last_speed_current_timestamps_ = {
+    {kChannel1, {}}, {kChannel2, {}}};
 
   std::mutex flags_current_timestamp_mtx_;
   timespec flags_current_timestamp_;
@@ -261,6 +155,43 @@ class RoboteqDriver : public RoboteqDriverInterface, public lely::canopen::LoopD
   timespec last_voltages_temps_timestamp_;
 
   const std::chrono::milliseconds sdo_operation_timeout_ms_;
+
+  std::map<std::string, std::shared_ptr<MotorDriver>> motor_drivers_;
+};
+
+class RoboteqMotorDriver : public MotorDriver
+{
+public:
+  RoboteqMotorDriver(std::shared_ptr<RoboteqDriver> driver, const std::uint8_t channel)
+  : driver_(driver), channel_(channel)
+  {
+  }
+
+  /**
+   * @brief Reads motor state data and saves last timestamps
+   */
+  MotorDriverState ReadMotorDriverState() override;
+
+  /**
+   * @brief Sends commands to the motors
+   *
+   * @param cmd command value in the range [-1000, 1000]
+   *
+   * @exception std::runtime_error if operation fails
+   */
+  void SendCmdVel(const std::int32_t cmd) override;
+
+  /**
+   * @brief Sends a safety stop command to the motor connected to channel 1
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  void TurnOnSafetyStop() override;
+
+private:
+  std::weak_ptr<RoboteqDriver> driver_;
+
+  const std::uint8_t channel_;
 };
 
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_data_converters.cpp b/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_data_converters.cpp
index d8d6991a..4c877913 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_data_converters.cpp
+++ b/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_data_converters.cpp
@@ -203,7 +203,7 @@ std::map<std::string, bool> RuntimeError::GetErrorMap() const
 }
 
 void RoboteqData::SetMotorsStates(
-  const RoboteqMotorState & left_state, const RoboteqMotorState & right_state,
+  const MotorDriverState & left_state, const MotorDriverState & right_state,
   const bool data_timed_out)
 {
   left_motor_state_.SetData(left_state);
@@ -211,7 +211,7 @@ void RoboteqData::SetMotorsStates(
   motor_states_data_timed_out_ = data_timed_out;
 }
 
-void RoboteqData::SetDriverState(const RoboteqDriverState & state, const bool data_timed_out)
+void RoboteqData::SetDriverState(const DriverState & state, const bool data_timed_out)
 {
   driver_state_.SetTemperature(state.mcu_temp);
   driver_state_.SetHeatsinkTemperature(state.heatsink_temp);
diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp b/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp
index ec9bf523..7b62a306 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp
@@ -41,17 +41,10 @@ struct CANopenObject
 // parameter names changed, but ids stayed the same, it will be better to just use ids directly
 struct RoboteqCANObjects
 {
-  static constexpr CANopenObject cmd_1 = {0x2000, 1};
-  static constexpr CANopenObject cmd_2 = {0x2000, 2};
-
-  static constexpr CANopenObject position_1 = {0x2104, 1};
-  static constexpr CANopenObject position_2 = {0x2104, 2};
-
-  static constexpr CANopenObject velocity_1 = {0x2107, 1};
-  static constexpr CANopenObject velocity_2 = {0x2107, 2};
-
-  static constexpr CANopenObject current_1 = {0x2100, 1};
-  static constexpr CANopenObject current_2 = {0x2100, 2};
+  static constexpr std::uint16_t cmd_id = 0x2000;
+  static constexpr std::uint16_t position_id = 0x2104;
+  static constexpr std::uint16_t velocity_id = 0x2107;
+  static constexpr std::uint16_t current_id = 0x2100;
 
   static constexpr CANopenObject flags = {0x2106, 7};
 
@@ -67,6 +60,44 @@ struct RoboteqCANObjects
   static constexpr CANopenObject turn_on_safety_stop = {0x202C, 0};  // Cmd_SFT
 };
 
+MotorDriverState RoboteqMotorDriver::ReadMotorDriverState()
+{
+  MotorDriverState state;
+
+  if (auto driver = driver_.lock()) {
+    state.pos = driver->rpdo_mapped[RoboteqCANObjects::position_id][channel_];
+    state.vel = driver->rpdo_mapped[RoboteqCANObjects::velocity_id][channel_];
+    state.current = driver->rpdo_mapped[RoboteqCANObjects::current_id][channel_];
+    state.pos_timestamp = driver->GetPositionTimestamp(channel_);
+    state.vel_current_timestamp = driver->GetSpeedCurrentTimestamp(channel_);
+  }
+
+  return state;
+}
+
+void RoboteqMotorDriver::SendCmdVel(const std::int32_t cmd)
+{
+  if (auto driver = driver_.lock()) {
+    driver->tpdo_mapped[RoboteqCANObjects::cmd_id][channel_] = cmd;
+    driver->tpdo_mapped[RoboteqCANObjects::cmd_id][channel_].WriteEvent();
+  }
+}
+
+void RoboteqMotorDriver::TurnOnSafetyStop()
+{
+  try {
+    if (auto driver = driver_.lock()) {
+      driver->SyncSDOWrite<std::uint8_t>(
+        RoboteqCANObjects::turn_on_safety_stop.id, RoboteqCANObjects::turn_on_safety_stop.subid,
+        channel_);
+    }
+  } catch (const std::runtime_error & e) {
+    throw std::runtime_error(
+      "Error when trying to turn on safety stop on channel " + std::to_string(channel_) + ": " +
+      std::string(e.what()));
+  }
+}
+
 RoboteqDriver::RoboteqDriver(
   const std::shared_ptr<lely::canopen::AsyncMaster> & master, const std::uint8_t id,
   const std::chrono::milliseconds & sdo_operation_timeout_ms)
@@ -85,42 +116,9 @@ std::future<void> RoboteqDriver::Boot()
   return future;
 }
 
-RoboteqMotorsStates RoboteqDriver::ReadRoboteqMotorsStates()
-{
-  RoboteqMotorsStates states;
-
-  // already does locking when accessing rpdo
-  states.motor_1.pos =
-    rpdo_mapped[RoboteqCANObjects::position_1.id][RoboteqCANObjects::position_1.subid];
-  states.motor_2.pos =
-    rpdo_mapped[RoboteqCANObjects::position_2.id][RoboteqCANObjects::position_2.subid];
-
-  states.motor_1.vel =
-    rpdo_mapped[RoboteqCANObjects::velocity_1.id][RoboteqCANObjects::velocity_1.subid];
-  states.motor_2.vel =
-    rpdo_mapped[RoboteqCANObjects::velocity_2.id][RoboteqCANObjects::velocity_2.subid];
-
-  states.motor_1.current =
-    rpdo_mapped[RoboteqCANObjects::current_1.id][RoboteqCANObjects::current_1.subid];
-  states.motor_2.current =
-    rpdo_mapped[RoboteqCANObjects::current_2.id][RoboteqCANObjects::current_2.subid];
-
-  {
-    std::lock_guard<std::mutex> lck_p(position_timestamp_mtx_);
-    states.pos_timestamp = last_position_timestamp_;
-  }
-
-  {
-    std::lock_guard<std::mutex> lck_sc(speed_current_timestamp_mtx_);
-    states.vel_current_timestamp = last_speed_current_timestamp_;
-  }
-
-  return states;
-}
-
-RoboteqDriverState RoboteqDriver::ReadRoboteqDriverState()
+DriverState RoboteqDriver::ReadDriverState()
 {
-  RoboteqDriverState state;
+  DriverState state;
 
   std::int32_t flags = static_cast<std::int32_t>(
     rpdo_mapped[RoboteqCANObjects::flags.id][RoboteqCANObjects::flags.subid]);
@@ -152,18 +150,7 @@ RoboteqDriverState RoboteqDriver::ReadRoboteqDriverState()
   return state;
 }
 
-void RoboteqDriver::SendRoboteqCmd(
-  const std::int32_t cmd_channel_1, const std::int32_t cmd_channel_2)
-{
-  tpdo_mapped[RoboteqCANObjects::cmd_1.id][RoboteqCANObjects::cmd_1.subid] = cmd_channel_1;
-  tpdo_mapped[RoboteqCANObjects::cmd_2.id][RoboteqCANObjects::cmd_2.subid] = cmd_channel_2;
-
-  // Both commands are in the TPDO 1, so write event for only one subid is triggered, as it will
-  // result in sending the whole TPDO 1 (both commands)
-  tpdo_mapped[RoboteqCANObjects::cmd_2.id][RoboteqCANObjects::cmd_2.subid].WriteEvent();
-}
-
-void RoboteqDriver::ResetRoboteqScript()
+void RoboteqDriver::ResetScript()
 {
   try {
     SyncSDOWrite<std::uint8_t>(
@@ -193,26 +180,23 @@ void RoboteqDriver::TurnOffEStop()
   }
 }
 
-void RoboteqDriver::TurnOnSafetyStopChannel1()
+void RoboteqDriver::AddMotorDriver(
+  const std::string name, std::shared_ptr<MotorDriver> motor_driver)
 {
-  try {
-    SyncSDOWrite<std::uint8_t>(
-      RoboteqCANObjects::turn_on_safety_stop.id, RoboteqCANObjects::turn_on_safety_stop.subid, 1);
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Error when trying to turn on safety stop on channel 1: " + std::string(e.what()));
+  if (std::dynamic_pointer_cast<RoboteqMotorDriver>(motor_driver) == nullptr) {
+    throw std::runtime_error("Motor driver is not of type RoboteqMotorDriver");
   }
+  motor_drivers_.emplace(name, motor_driver);
 }
 
-void RoboteqDriver::TurnOnSafetyStopChannel2()
+std::shared_ptr<MotorDriver> RoboteqDriver::GetMotorDriver(const std::string & name)
 {
-  try {
-    SyncSDOWrite<std::uint8_t>(
-      RoboteqCANObjects::turn_on_safety_stop.id, RoboteqCANObjects::turn_on_safety_stop.subid, 2);
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Error when trying to turn on safety stop on channel 2: " + std::string(e.what()));
+  auto it = motor_drivers_.find(name);
+  if (it == motor_drivers_.end()) {
+    throw std::runtime_error("Motor driver with name '" + name + "' does not exist");
   }
+
+  return it->second;
 }
 
 template <typename T>
@@ -270,13 +254,18 @@ void RoboteqDriver::OnBoot(
 
 void RoboteqDriver::OnRpdoWrite(const std::uint16_t idx, const std::uint8_t subidx) noexcept
 {
-  if (idx == RoboteqCANObjects::position_1.id && subidx == RoboteqCANObjects::position_1.subid) {
+  if (idx == RoboteqCANObjects::position_id) {
+    if (subidx != kChannel1 && subidx != kChannel2) {
+      return;
+    }
     std::lock_guard<std::mutex> lck(position_timestamp_mtx_);
-    clock_gettime(CLOCK_MONOTONIC, &last_position_timestamp_);
-  } else if (
-    idx == RoboteqCANObjects::velocity_1.id && subidx == RoboteqCANObjects::velocity_1.subid) {
+    clock_gettime(CLOCK_MONOTONIC, &last_position_timestamps_.at(subidx));
+  } else if (idx == RoboteqCANObjects::velocity_id) {
+    if (subidx != kChannel1 && subidx != kChannel2) {
+      return;
+    }
     std::lock_guard<std::mutex> lck(speed_current_timestamp_mtx_);
-    clock_gettime(CLOCK_MONOTONIC, &last_speed_current_timestamp_);
+    clock_gettime(CLOCK_MONOTONIC, &last_speed_current_timestamps_.at(subidx));
   } else if (idx == RoboteqCANObjects::flags.id && subidx == RoboteqCANObjects::flags.subid) {
     std::lock_guard<std::mutex> lck(flags_current_timestamp_mtx_);
     clock_gettime(CLOCK_MONOTONIC, &flags_current_timestamp_);
diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
index 1a4463d0..624d9862 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
@@ -47,6 +47,14 @@ class TestRoboteqDriver : public ::testing::Test
 
     roboteq_driver_ = std::make_shared<panther_hardware_interfaces::RoboteqDriver>(
       canopen_controller_->GetMaster(), 1, std::chrono::milliseconds(100));
+
+    auto motor_1 = std::make_shared<panther_hardware_interfaces::RoboteqMotorDriver>(
+      roboteq_driver_, panther_hardware_interfaces::RoboteqDriver::kChannel1);
+    auto motor_2 = std::make_shared<panther_hardware_interfaces::RoboteqMotorDriver>(
+      roboteq_driver_, panther_hardware_interfaces::RoboteqDriver::kChannel2);
+
+    roboteq_driver_->AddMotorDriver(kMotor1Name, motor_1);
+    roboteq_driver_->AddMotorDriver(kMotor2Name, motor_2);
     roboteq_driver_->Boot();
   }
 
@@ -60,81 +68,88 @@ class TestRoboteqDriver : public ::testing::Test
   }
 
 protected:
+  static constexpr char kMotor1Name[] = "motor_1";
+  static constexpr char kMotor2Name[] = "motor_2";
+
   std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
   std::unique_ptr<panther_hardware_interfaces_test::RoboteqsMock> roboteqs_mock_;
   std::unique_ptr<panther_hardware_interfaces::CANopenController> canopen_controller_;
-  panther_hardware_interfaces::RoboteqDriverInterface::SharedPtr roboteq_driver_;
+  std::shared_ptr<panther_hardware_interfaces::RoboteqDriver> roboteq_driver_;
 };
 
 TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
 {
   using panther_hardware_interfaces_test::DriverChannel;
 
-  const std::int32_t left_pos = 101;
-  const std::int32_t left_vel = 102;
-  const std::int32_t left_current = 103;
-  const std::int32_t right_pos = 201;
-  const std::int32_t right_vel = 202;
-  const std::int32_t right_current = 203;
+  const std::int32_t motor_1_pos = 101;
+  const std::int32_t motor_1_vel = 102;
+  const std::int32_t motor_1_current = 103;
+  const std::int32_t motor_2_pos = 201;
+  const std::int32_t motor_2_vel = 202;
+  const std::int32_t motor_2_current = 203;
 
-  roboteqs_mock_->GetDriver()->SetPosition(DriverChannel::CHANNEL2, left_pos);
-  roboteqs_mock_->GetDriver()->SetPosition(DriverChannel::CHANNEL1, right_pos);
+  roboteqs_mock_->GetDriver()->SetPosition(DriverChannel::CHANNEL1, motor_1_pos);
+  roboteqs_mock_->GetDriver()->SetPosition(DriverChannel::CHANNEL2, motor_2_pos);
 
-  roboteqs_mock_->GetDriver()->SetVelocity(DriverChannel::CHANNEL2, left_vel);
-  roboteqs_mock_->GetDriver()->SetVelocity(DriverChannel::CHANNEL1, right_vel);
+  roboteqs_mock_->GetDriver()->SetVelocity(DriverChannel::CHANNEL1, motor_1_vel);
+  roboteqs_mock_->GetDriver()->SetVelocity(DriverChannel::CHANNEL2, motor_2_vel);
 
-  roboteqs_mock_->GetDriver()->SetCurrent(DriverChannel::CHANNEL2, left_current);
-  roboteqs_mock_->GetDriver()->SetCurrent(DriverChannel::CHANNEL1, right_current);
+  roboteqs_mock_->GetDriver()->SetCurrent(DriverChannel::CHANNEL1, motor_1_current);
+  roboteqs_mock_->GetDriver()->SetCurrent(DriverChannel::CHANNEL2, motor_2_current);
 
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  panther_hardware_interfaces::RoboteqMotorsStates fb =
-    roboteq_driver_->ReadRoboteqMotorsStates();
+  panther_hardware_interfaces::MotorDriverState fb_motor_1 =
+    roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadMotorDriverState();
+  panther_hardware_interfaces::MotorDriverState fb_motor_2 =
+    roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadMotorDriverState();
 
-  EXPECT_EQ(fb.motor_2.pos, left_pos);
-  EXPECT_EQ(fb.motor_2.vel, left_vel);
-  EXPECT_EQ(fb.motor_2.current, left_current);
+  EXPECT_EQ(fb_motor_1.pos, motor_1_pos);
+  EXPECT_EQ(fb_motor_1.vel, motor_1_vel);
+  EXPECT_EQ(fb_motor_1.current, motor_1_current);
 
-  EXPECT_EQ(fb.motor_1.pos, right_pos);
-  EXPECT_EQ(fb.motor_1.vel, right_vel);
-  EXPECT_EQ(fb.motor_1.current, right_current);
+  EXPECT_EQ(fb_motor_2.pos, motor_2_pos);
+  EXPECT_EQ(fb_motor_2.vel, motor_2_vel);
+  EXPECT_EQ(fb_motor_2.current, motor_2_current);
 }
 
 TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps)
 {
   std::this_thread::sleep_for(std::chrono::milliseconds(150));
 
-  panther_hardware_interfaces::RoboteqMotorsStates fb1 =
-    roboteq_driver_->ReadRoboteqMotorsStates();
+  panther_hardware_interfaces::MotorDriverState fb_motor_1 =
+    roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadMotorDriverState();
 
   // based on publishing frequency in the Roboteq mock (10)
   std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
-  panther_hardware_interfaces::RoboteqMotorsStates fb2 =
-    roboteq_driver_->ReadRoboteqMotorsStates();
+  panther_hardware_interfaces::MotorDriverState fb_motor_2 =
+    roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadMotorDriverState();
 
   // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
   // if consecutive messages will have timestamps 100ms + some threshold apart
   EXPECT_LE(
-    lely::util::from_timespec(fb2.pos_timestamp) - lely::util::from_timespec(fb1.pos_timestamp),
+    lely::util::from_timespec(fb_motor_2.pos_timestamp) -
+      lely::util::from_timespec(fb_motor_1.pos_timestamp),
     std::chrono::milliseconds(15));
 
   EXPECT_GE(
-    lely::util::from_timespec(fb2.pos_timestamp) - lely::util::from_timespec(fb1.pos_timestamp),
+    lely::util::from_timespec(fb_motor_2.pos_timestamp) -
+      lely::util::from_timespec(fb_motor_1.pos_timestamp),
     std::chrono::milliseconds(5));
 
   EXPECT_LE(
-    lely::util::from_timespec(fb2.vel_current_timestamp) -
-      lely::util::from_timespec(fb1.vel_current_timestamp),
+    lely::util::from_timespec(fb_motor_2.vel_current_timestamp) -
+      lely::util::from_timespec(fb_motor_1.vel_current_timestamp),
     std::chrono::milliseconds(15));
 
   EXPECT_GE(
-    lely::util::from_timespec(fb2.vel_current_timestamp) -
-      lely::util::from_timespec(fb1.vel_current_timestamp),
+    lely::util::from_timespec(fb_motor_2.vel_current_timestamp) -
+      lely::util::from_timespec(fb_motor_1.vel_current_timestamp),
     std::chrono::milliseconds(5));
 }
 
-TEST_F(TestRoboteqDriver, ReadRoboteqDriverState)
+TEST_F(TestRoboteqDriver, ReadDriverState)
 {
   using panther_hardware_interfaces_test::DriverChannel;
   using panther_hardware_interfaces_test::DriverFaultFlags;
@@ -162,8 +177,7 @@ TEST_F(TestRoboteqDriver, ReadRoboteqDriverState)
 
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  panther_hardware_interfaces::RoboteqDriverState fb =
-    roboteq_driver_->ReadRoboteqDriverState();
+  panther_hardware_interfaces::DriverState fb = roboteq_driver_->ReadDriverState();
 
   EXPECT_EQ(fb.mcu_temp, temp);
   EXPECT_EQ(fb.heatsink_temp, heatsink_temp);
@@ -177,18 +191,16 @@ TEST_F(TestRoboteqDriver, ReadRoboteqDriverState)
   EXPECT_EQ(fb.runtime_stat_flag_motor_2, 0b00001000);
 }
 
-TEST_F(TestRoboteqDriver, ReadRoboteqDriverStateTimestamp)
+TEST_F(TestRoboteqDriver, ReadDriverStateTimestamp)
 {
   std::this_thread::sleep_for(std::chrono::milliseconds(150));
 
-  panther_hardware_interfaces::RoboteqDriverState fb1 =
-    roboteq_driver_->ReadRoboteqDriverState();
+  panther_hardware_interfaces::DriverState fb1 = roboteq_driver_->ReadDriverState();
 
   // based on publishing frequency in the Roboteq mock (50)
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  panther_hardware_interfaces::RoboteqDriverState fb2 =
-    roboteq_driver_->ReadRoboteqDriverState();
+  panther_hardware_interfaces::DriverState fb2 = roboteq_driver_->ReadDriverState();
 
   // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
   // if consecutive messages will have timestamps 100ms + some threshold apart
@@ -217,22 +229,23 @@ TEST_F(TestRoboteqDriver, SendRoboteqCmd)
 {
   using panther_hardware_interfaces_test::DriverChannel;
 
-  const std::int32_t left_v = 10;
-  const std::int32_t right_v = 20;
+  const std::int32_t motor_1_v = 10;
+  const std::int32_t motor_2_v = 20;
 
-  roboteq_driver_->SendRoboteqCmd(right_v, left_v);
+  roboteq_driver_->GetMotorDriver(kMotor1Name)->SendCmdVel(motor_1_v);
+  roboteq_driver_->GetMotorDriver(kMotor2Name)->SendCmdVel(motor_2_v);
 
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), left_v);
-  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), right_v);
+  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), motor_1_v);
+  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), motor_2_v);
 }
 
 TEST_F(TestRoboteqDriver, ResetRoboteqScript)
 {
   std::this_thread::sleep_for(std::chrono::milliseconds(150));
   roboteqs_mock_->GetDriver()->SetResetRoboteqScript(65);
-  roboteq_driver_->ResetRoboteqScript();
+  roboteq_driver_->ResetScript();
 
   EXPECT_EQ(roboteqs_mock_->GetDriver()->GetResetRoboteqScript(), 2);
 }
@@ -259,7 +272,7 @@ TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel1)
 {
   std::this_thread::sleep_for(std::chrono::milliseconds(150));
   roboteqs_mock_->GetDriver()->SetTurnOnSafetyStop(67);
-  roboteq_driver_->TurnOnSafetyStopChannel1();
+  roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop();
 
   EXPECT_EQ(roboteqs_mock_->GetDriver()->GetTurnOnSafetyStop(), 1);
 }
@@ -268,7 +281,7 @@ TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel2)
 {
   std::this_thread::sleep_for(std::chrono::milliseconds(150));
   roboteqs_mock_->GetDriver()->SetTurnOnSafetyStop(65);
-  roboteq_driver_->TurnOnSafetyStopChannel2();
+  roboteq_driver_->GetMotorDriver(kMotor2Name)->TurnOnSafetyStop();
 
   EXPECT_EQ(roboteqs_mock_->GetDriver()->GetTurnOnSafetyStop(), 2);
 }
@@ -278,7 +291,7 @@ TEST_F(TestRoboteqDriver, WriteTimeout)
   std::this_thread::sleep_for(std::chrono::milliseconds(150));
   roboteqs_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 200000);
   ASSERT_THROW(
-    roboteq_driver_->TurnOnSafetyStopChannel1(), std::runtime_error);
+    roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop(), std::runtime_error);
 }
 
 // OnCanError isn't tested, because it reacts to lower-level CAN errors (CRC), which are hard to
diff --git a/panther_hardware_interfaces/test/utils/fake_can_socket.hpp b/panther_hardware_interfaces/test/utils/fake_can_socket.hpp
index e9ed9c82..e2920cff 100644
--- a/panther_hardware_interfaces/test/utils/fake_can_socket.hpp
+++ b/panther_hardware_interfaces/test/utils/fake_can_socket.hpp
@@ -77,4 +77,4 @@ class FakeCANSocket
 
 }  // namespace panther_hardware_interfaces_test
 
-#endif  // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_
\ No newline at end of file
+#endif  // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_

From 860c0135a1eed2c45cb1695e61c36e1dfcfd2d5d Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Fri, 23 Aug 2024 06:26:19 +0000
Subject: [PATCH 013/100] rename canopen controller to manager

---
 panther_hardware_interfaces/CMakeLists.txt    | 18 +++++-----
 ...pen_controller.hpp => canopen_manager.hpp} | 14 ++++----
 ...pen_controller.cpp => canopen_manager.cpp} | 12 +++----
 ...ontroller.cpp => test_canopen_manager.cpp} | 34 +++++++++----------
 .../motors_controller/test_roboteq_driver.cpp | 12 +++----
 .../test/utils/test_constants.hpp             |  2 +-
 6 files changed, 46 insertions(+), 46 deletions(-)
 rename panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/{canopen_controller.hpp => canopen_manager.hpp} (91%)
 rename panther_hardware_interfaces/src/panther_system/motors_controller/{canopen_controller.cpp => canopen_manager.cpp} (93%)
 rename panther_hardware_interfaces/test/unit/panther_system/motors_controller/{test_canopen_controller.cpp => test_canopen_manager.cpp} (61%)

diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt
index fa48f2a5..b87fda02 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/panther_hardware_interfaces/CMakeLists.txt
@@ -55,7 +55,7 @@ add_library(
   src/panther_imu_sensor/panther_imu_sensor.cpp
   src/panther_system/gpio/gpio_controller.cpp
   src/panther_system/gpio/gpio_driver.cpp
-  src/panther_system/motors_controller/canopen_controller.cpp
+  src/panther_system/motors_controller/canopen_manager.cpp
   src/panther_system/motors_controller/motors_controller.cpp
   src/panther_system/motors_controller/roboteq_data_converters.cpp
   src/panther_system/motors_controller/roboteq_driver.cpp
@@ -118,24 +118,24 @@ if(BUILD_TESTING)
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gtest(
-    ${PROJECT_NAME}_test_canopen_controller
-    test/unit/panther_system/motors_controller/test_canopen_controller.cpp
-    src/panther_system/motors_controller/canopen_controller.cpp
+    ${PROJECT_NAME}_test_canopen_manager
+    test/unit/panther_system/motors_controller/test_canopen_manager.cpp
+    src/panther_system/motors_controller/canopen_manager.cpp
     src/panther_system/motors_controller/roboteq_driver.cpp
     src/utils.cpp)
   target_include_directories(
-    ${PROJECT_NAME}_test_canopen_controller
+    ${PROJECT_NAME}_test_canopen_manager
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
-  ament_target_dependencies(${PROJECT_NAME}_test_canopen_controller rclcpp
+  ament_target_dependencies(${PROJECT_NAME}_test_canopen_manager rclcpp
                             panther_msgs panther_utils)
-  target_link_libraries(${PROJECT_NAME}_test_canopen_controller
+  target_link_libraries(${PROJECT_NAME}_test_canopen_manager
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_driver
     test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
-    src/panther_system/motors_controller/canopen_controller.cpp
+    src/panther_system/motors_controller/canopen_manager.cpp
     src/panther_system/motors_controller/roboteq_driver.cpp
     src/utils.cpp)
   target_include_directories(
@@ -150,7 +150,7 @@ if(BUILD_TESTING)
   ament_add_gtest(
     ${PROJECT_NAME}_test_motors_controller
     test/panther_system/motors_controller/test_motors_controller.cpp
-    src/panther_system/motors_controller/canopen_controller.cpp
+    src/panther_system/motors_controller/canopen_manager.cpp
     src/panther_system/motors_controller/roboteq_driver.cpp
     src/panther_system/motors_controller/roboteq_data_converters.cpp
     src/panther_system/motors_controller/motors_controller.cpp
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp
similarity index 91%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp
rename to panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp
index 53ba4292..fb9721c7 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_CONTROLLER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_CONTROLLER_HPP_
+#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_MANAGER_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_MANAGER_HPP_
 
 #include <atomic>
 #include <chrono>
@@ -50,15 +50,15 @@ struct CANopenSettings
 };
 
 /**
- * @brief CANopenController takes care of CANopen communication - creates master controller
+ * @brief CANopenManager takes care of CANopen communication - creates master controller
  * and two Roboteq drivers (front and rear)
  */
-class CANopenController
+class CANopenManager
 {
 public:
-  CANopenController(const CANopenSettings & canopen_settings);
+  CANopenManager(const CANopenSettings & canopen_settings);
 
-  ~CANopenController() { Deinitialize(); }
+  ~CANopenManager() { Deinitialize(); }
 
   /**
    * @brief Starts CANopen communication (in a new thread) and waits for boot to finish
@@ -110,4 +110,4 @@ class CANopenController
 
 }  // namespace panther_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_CONTROLLER_HPP_
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_MANAGER_HPP_
diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp b/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_manager.cpp
similarity index 93%
rename from panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp
rename to panther_hardware_interfaces/src/panther_system/motors_controller/canopen_manager.cpp
index 50cfc9d4..82573100 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp
+++ b/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_manager.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp"
+#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp"
 
 #include <condition_variable>
 #include <filesystem>
@@ -29,12 +29,12 @@
 namespace panther_hardware_interfaces
 {
 
-CANopenController::CANopenController(const CANopenSettings & canopen_settings)
+CANopenManager::CANopenManager(const CANopenSettings & canopen_settings)
 : canopen_settings_(canopen_settings)
 {
 }
 
-void CANopenController::Initialize()
+void CANopenManager::Initialize()
 {
   if (initialized_) {
     return;
@@ -83,7 +83,7 @@ void CANopenController::Initialize()
   initialized_ = true;
 }
 
-void CANopenController::Deinitialize()
+void CANopenManager::Deinitialize()
 {
   // Deinitialization should be done regardless of the initialized_ state - in case some operation
   // during initialization fails, it is still necessary to do the cleanup
@@ -110,7 +110,7 @@ void CANopenController::Deinitialize()
   initialized_ = false;
 }
 
-void CANopenController::InitializeCANCommunication()
+void CANopenManager::InitializeCANCommunication()
 {
   lely::io::IoGuard io_guard;
 
@@ -140,7 +140,7 @@ void CANopenController::InitializeCANCommunication()
   master_->Reset();
 }
 
-void CANopenController::NotifyCANCommunicationStarted(const bool result)
+void CANopenManager::NotifyCANCommunicationStarted(const bool result)
 {
   {
     std::lock_guard<std::mutex> lck_g(canopen_communication_started_mtx_);
diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_controller.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp
similarity index 61%
rename from panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_controller.cpp
rename to panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp
index e1833be8..f69e3e70 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_controller.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp
@@ -19,53 +19,53 @@
 #include <gtest/gtest.h>
 #include <sys/socket.h>
 
-#include <panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp>
+#include <panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp>
 
 #include "utils/fake_can_socket.hpp"
 #include "utils/test_constants.hpp"
 
-class TestCANopenController : public ::testing::Test
+class TestCANopenManager : public ::testing::Test
 {
 public:
-  TestCANopenController();
+  TestCANopenManager();
 
-  ~TestCANopenController() {}
+  ~TestCANopenManager() {}
 
 protected:
   std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
-  std::unique_ptr<panther_hardware_interfaces::CANopenController> canopen_controller_;
+  std::unique_ptr<panther_hardware_interfaces::CANopenManager> canopen_manager_;
 };
 
-TestCANopenController::TestCANopenController()
+TestCANopenManager::TestCANopenManager()
 {
   can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
     panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
 
-  canopen_controller_ = std::make_unique<panther_hardware_interfaces::CANopenController>(
+  canopen_manager_ = std::make_unique<panther_hardware_interfaces::CANopenManager>(
     panther_hardware_interfaces_test::kCANopenSettings);
 }
 
-TEST_F(TestCANopenController, InitializeAndDeinitialize)
+TEST_F(TestCANopenManager, InitializeAndDeinitialize)
 {
   can_socket_->Initialize();
 
-  ASSERT_NO_THROW(canopen_controller_->Initialize());
-  ASSERT_NO_THROW(canopen_controller_->Deinitialize());
+  ASSERT_NO_THROW(canopen_manager_->Initialize());
+  ASSERT_NO_THROW(canopen_manager_->Deinitialize());
 
   // Check if deinitialization worked correctly - initialize once again
-  ASSERT_NO_THROW(canopen_controller_->Initialize());
-  ASSERT_NO_THROW(canopen_controller_->Deinitialize());
+  ASSERT_NO_THROW(canopen_manager_->Initialize());
+  ASSERT_NO_THROW(canopen_manager_->Deinitialize());
 }
 
-TEST_F(TestCANopenController, InitializeWithError)
+TEST_F(TestCANopenManager, InitializeWithError)
 {
   // CAN socket not initialized, should throw
-  ASSERT_THROW(canopen_controller_->Initialize(), std::runtime_error);
-  ASSERT_NO_THROW(canopen_controller_->Deinitialize());
+  ASSERT_THROW(canopen_manager_->Initialize(), std::runtime_error);
+  ASSERT_NO_THROW(canopen_manager_->Deinitialize());
 
   can_socket_->Initialize();
-  ASSERT_NO_THROW(canopen_controller_->Initialize());
-  ASSERT_NO_THROW(canopen_controller_->Deinitialize());
+  ASSERT_NO_THROW(canopen_manager_->Initialize());
+  ASSERT_NO_THROW(canopen_manager_->Deinitialize());
 }
 
 int main(int argc, char ** argv)
diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
index 624d9862..c70d47b5 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
@@ -20,7 +20,7 @@
 
 #include <gtest/gtest.h>
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp"
+#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp"
 #include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp"
 
 #include "utils/fake_can_socket.hpp"
@@ -38,15 +38,15 @@ class TestRoboteqDriver : public ::testing::Test
       panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
     can_socket_->Initialize();
 
-    canopen_controller_ = std::make_unique<panther_hardware_interfaces::CANopenController>(
+    canopen_manager_ = std::make_unique<panther_hardware_interfaces::CANopenManager>(
       panther_hardware_interfaces_test::kCANopenSettings);
 
     roboteqs_mock_ = std::make_unique<panther_hardware_interfaces_test::RoboteqsMock>();
     roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
-    canopen_controller_->Initialize();
+    canopen_manager_->Initialize();
 
     roboteq_driver_ = std::make_shared<panther_hardware_interfaces::RoboteqDriver>(
-      canopen_controller_->GetMaster(), 1, std::chrono::milliseconds(100));
+      canopen_manager_->GetMaster(), 1, std::chrono::milliseconds(100));
 
     auto motor_1 = std::make_shared<panther_hardware_interfaces::RoboteqMotorDriver>(
       roboteq_driver_, panther_hardware_interfaces::RoboteqDriver::kChannel1);
@@ -61,7 +61,7 @@ class TestRoboteqDriver : public ::testing::Test
   ~TestRoboteqDriver()
   {
     roboteq_driver_.reset();
-    canopen_controller_->Deinitialize();
+    canopen_manager_->Deinitialize();
     roboteqs_mock_->Stop();
     roboteqs_mock_.reset();
     can_socket_->Deinitialize();
@@ -73,7 +73,7 @@ class TestRoboteqDriver : public ::testing::Test
 
   std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
   std::unique_ptr<panther_hardware_interfaces_test::RoboteqsMock> roboteqs_mock_;
-  std::unique_ptr<panther_hardware_interfaces::CANopenController> canopen_controller_;
+  std::unique_ptr<panther_hardware_interfaces::CANopenManager> canopen_manager_;
   std::shared_ptr<panther_hardware_interfaces::RoboteqDriver> roboteq_driver_;
 };
 
diff --git a/panther_hardware_interfaces/test/utils/test_constants.hpp b/panther_hardware_interfaces/test/utils/test_constants.hpp
index 92e4fe83..380f2e5a 100644
--- a/panther_hardware_interfaces/test/utils/test_constants.hpp
+++ b/panther_hardware_interfaces/test/utils/test_constants.hpp
@@ -21,7 +21,7 @@
 #include <string>
 #include <vector>
 
-#include <panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp>
+#include <panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp>
 #include <panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp>
 
 namespace panther_hardware_interfaces_test

From 3df7dee4f6b58bbf7a7b5a9823a0c98ea2c54212 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Fri, 23 Aug 2024 06:39:48 +0000
Subject: [PATCH 014/100] small fixes

---
 .../panther_system/motors_controller/roboteq_driver.hpp     | 6 +++---
 .../motors_controller/test_canopen_manager.cpp              | 2 +-
 .../motors_controller/test_roboteq_driver.cpp               | 2 --
 3 files changed, 4 insertions(+), 6 deletions(-)

diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
index 0bd37821..05859405 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
@@ -103,9 +103,6 @@ class RoboteqDriver : public Driver, public lely::canopen::LoopDriver
   template <typename T>
   void SyncSDOWrite(const std::uint16_t index, const std::uint8_t subindex, const T data);
 
-  static constexpr std::uint8_t kChannel1 = 1;
-  static constexpr std::uint8_t kChannel2 = 2;
-
   /**
    * @brief Returns the last timestamp of the position data for the given channel
    */
@@ -124,6 +121,9 @@ class RoboteqDriver : public Driver, public lely::canopen::LoopDriver
     return last_speed_current_timestamps_.at(channel);
   }
 
+  static constexpr std::uint8_t kChannel1 = 1;
+  static constexpr std::uint8_t kChannel2 = 2;
+
 private:
   void OnBoot(
     const lely::canopen::NmtState st, const char es, const std::string & what) noexcept override;
diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp
index f69e3e70..636956f3 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp
@@ -19,7 +19,7 @@
 #include <gtest/gtest.h>
 #include <sys/socket.h>
 
-#include <panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp>
+#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp"
 
 #include "utils/fake_can_socket.hpp"
 #include "utils/test_constants.hpp"
diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
index c70d47b5..7f49a7df 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
@@ -27,8 +27,6 @@
 #include "utils/roboteqs_mock.hpp"
 #include "utils/test_constants.hpp"
 
-#include <iostream>
-
 class TestRoboteqDriver : public ::testing::Test
 {
 public:

From f046a75d02e6c4e5b505455ee9729cbfe5a95260 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Fri, 23 Aug 2024 08:14:42 +0000
Subject: [PATCH 015/100] add roboteq driver boot tests

---
 .../motors_controller/roboteq_driver.cpp      |   1 +
 .../motors_controller/test_roboteq_driver.cpp | 122 +++++++++++++-----
 2 files changed, 90 insertions(+), 33 deletions(-)

diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp b/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp
index 7b62a306..d563a495 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp
@@ -108,6 +108,7 @@ RoboteqDriver::RoboteqDriver(
 std::future<void> RoboteqDriver::Boot()
 {
   std::lock_guard<std::mutex> lck(boot_mtx_);
+  boot_promise_ = std::promise<void>();
   std::future<void> future = boot_promise_.get_future();
 
   if (!LoopDriver::Boot()) {
diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
index 7f49a7df..c9e96433 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
@@ -30,42 +30,15 @@
 class TestRoboteqDriver : public ::testing::Test
 {
 public:
-  TestRoboteqDriver()
-  {
-    can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
-      panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
-    can_socket_->Initialize();
-
-    canopen_manager_ = std::make_unique<panther_hardware_interfaces::CANopenManager>(
-      panther_hardware_interfaces_test::kCANopenSettings);
-
-    roboteqs_mock_ = std::make_unique<panther_hardware_interfaces_test::RoboteqsMock>();
-    roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
-    canopen_manager_->Initialize();
-
-    roboteq_driver_ = std::make_shared<panther_hardware_interfaces::RoboteqDriver>(
-      canopen_manager_->GetMaster(), 1, std::chrono::milliseconds(100));
-
-    auto motor_1 = std::make_shared<panther_hardware_interfaces::RoboteqMotorDriver>(
-      roboteq_driver_, panther_hardware_interfaces::RoboteqDriver::kChannel1);
-    auto motor_2 = std::make_shared<panther_hardware_interfaces::RoboteqMotorDriver>(
-      roboteq_driver_, panther_hardware_interfaces::RoboteqDriver::kChannel2);
-
-    roboteq_driver_->AddMotorDriver(kMotor1Name, motor_1);
-    roboteq_driver_->AddMotorDriver(kMotor2Name, motor_2);
-    roboteq_driver_->Boot();
-  }
+  TestRoboteqDriver();
 
-  ~TestRoboteqDriver()
-  {
-    roboteq_driver_.reset();
-    canopen_manager_->Deinitialize();
-    roboteqs_mock_->Stop();
-    roboteqs_mock_.reset();
-    can_socket_->Deinitialize();
-  }
+  ~TestRoboteqDriver();
+
+  void BootRoboteqDriver();
 
 protected:
+  bool TestBoot();
+
   static constexpr char kMotor1Name[] = "motor_1";
   static constexpr char kMotor2Name[] = "motor_2";
 
@@ -75,6 +48,82 @@ class TestRoboteqDriver : public ::testing::Test
   std::shared_ptr<panther_hardware_interfaces::RoboteqDriver> roboteq_driver_;
 };
 
+TestRoboteqDriver::TestRoboteqDriver()
+{
+  can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
+    panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
+  can_socket_->Initialize();
+
+  canopen_manager_ = std::make_unique<panther_hardware_interfaces::CANopenManager>(
+    panther_hardware_interfaces_test::kCANopenSettings);
+
+  roboteqs_mock_ = std::make_unique<panther_hardware_interfaces_test::RoboteqsMock>();
+  roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
+  canopen_manager_->Initialize();
+
+  roboteq_driver_ = std::make_shared<panther_hardware_interfaces::RoboteqDriver>(
+    canopen_manager_->GetMaster(), 1, std::chrono::milliseconds(100));
+}
+
+TestRoboteqDriver::~TestRoboteqDriver()
+{
+  roboteq_driver_.reset();
+  canopen_manager_->Deinitialize();
+  roboteqs_mock_->Stop();
+  roboteqs_mock_.reset();
+  can_socket_->Deinitialize();
+}
+
+void TestRoboteqDriver::BootRoboteqDriver()
+{
+  auto motor_1 = std::make_shared<panther_hardware_interfaces::RoboteqMotorDriver>(
+    roboteq_driver_, panther_hardware_interfaces::RoboteqDriver::kChannel1);
+  auto motor_2 = std::make_shared<panther_hardware_interfaces::RoboteqMotorDriver>(
+    roboteq_driver_, panther_hardware_interfaces::RoboteqDriver::kChannel2);
+
+  roboteq_driver_->AddMotorDriver(kMotor1Name, motor_1);
+  roboteq_driver_->AddMotorDriver(kMotor2Name, motor_2);
+  roboteq_driver_->Boot();
+}
+
+bool TestRoboteqDriver::TestBoot()
+{
+  auto future = roboteq_driver_->Boot();
+  const auto future_status = future.wait_for(std::chrono::milliseconds(500));
+
+  if (future_status != std::future_status::ready) {
+    return false;
+  }
+
+  try {
+    future.get();
+  } catch (const std::exception & e) {
+    return false;
+  }
+
+  return true;
+}
+
+TEST_F(TestRoboteqDriver, BootRoboteqDriver) { ASSERT_TRUE(TestBoot()); }
+
+TEST_F(TestRoboteqDriver, BootErrorDeviceType)
+{
+  roboteqs_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(0x1000, 0, 100000);
+  ASSERT_FALSE(TestBoot());
+
+  roboteqs_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(0x1000, 0, 0);
+  ASSERT_TRUE(TestBoot());
+}
+
+TEST_F(TestRoboteqDriver, BootErrorVendorId)
+{
+  roboteqs_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(0x1018, 1, 100000);
+  ASSERT_FALSE(TestBoot());
+
+  roboteqs_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(0x1018, 1, 0);
+  ASSERT_TRUE(TestBoot());
+}
+
 TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
 {
   using panther_hardware_interfaces_test::DriverChannel;
@@ -86,6 +135,8 @@ TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
   const std::int32_t motor_2_vel = 202;
   const std::int32_t motor_2_current = 203;
 
+  BootRoboteqDriver();
+
   roboteqs_mock_->GetDriver()->SetPosition(DriverChannel::CHANNEL1, motor_1_pos);
   roboteqs_mock_->GetDriver()->SetPosition(DriverChannel::CHANNEL2, motor_2_pos);
 
@@ -113,6 +164,7 @@ TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
 
 TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps)
 {
+  BootRoboteqDriver();
   std::this_thread::sleep_for(std::chrono::milliseconds(150));
 
   panther_hardware_interfaces::MotorDriverState fb_motor_1 =
@@ -230,6 +282,8 @@ TEST_F(TestRoboteqDriver, SendRoboteqCmd)
   const std::int32_t motor_1_v = 10;
   const std::int32_t motor_2_v = 20;
 
+  BootRoboteqDriver();
+
   roboteq_driver_->GetMotorDriver(kMotor1Name)->SendCmdVel(motor_1_v);
   roboteq_driver_->GetMotorDriver(kMotor2Name)->SendCmdVel(motor_2_v);
 
@@ -268,6 +322,7 @@ TEST_F(TestRoboteqDriver, TurnOffEStop)
 
 TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel1)
 {
+  BootRoboteqDriver();
   std::this_thread::sleep_for(std::chrono::milliseconds(150));
   roboteqs_mock_->GetDriver()->SetTurnOnSafetyStop(67);
   roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop();
@@ -277,6 +332,7 @@ TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel1)
 
 TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel2)
 {
+  BootRoboteqDriver();
   std::this_thread::sleep_for(std::chrono::milliseconds(150));
   roboteqs_mock_->GetDriver()->SetTurnOnSafetyStop(65);
   roboteq_driver_->GetMotorDriver(kMotor2Name)->TurnOnSafetyStop();

From bf2b230feb65e9bd449ce7cab64e41c13520fb92 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Fri, 23 Aug 2024 09:36:38 +0000
Subject: [PATCH 016/100] update roboteq tests

---
 .../motors_controller/test_roboteq_driver.cpp | 57 ++++++++++++++-----
 .../test/utils/roboteqs_mock.hpp              |  2 +-
 2 files changed, 45 insertions(+), 14 deletions(-)

diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
index c9e96433..022662bf 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
@@ -27,6 +27,8 @@
 #include "utils/roboteqs_mock.hpp"
 #include "utils/test_constants.hpp"
 
+#include "panther_utils/test/test_utils.hpp"
+
 class TestRoboteqDriver : public ::testing::Test
 {
 public:
@@ -83,7 +85,8 @@ void TestRoboteqDriver::BootRoboteqDriver()
 
   roboteq_driver_->AddMotorDriver(kMotor1Name, motor_1);
   roboteq_driver_->AddMotorDriver(kMotor2Name, motor_2);
-  roboteq_driver_->Boot();
+  auto future = roboteq_driver_->Boot();
+  future.wait();
 }
 
 bool TestRoboteqDriver::TestBoot()
@@ -111,6 +114,7 @@ TEST_F(TestRoboteqDriver, BootErrorDeviceType)
   roboteqs_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(0x1000, 0, 100000);
   ASSERT_FALSE(TestBoot());
 
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
   roboteqs_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(0x1000, 0, 0);
   ASSERT_TRUE(TestBoot());
 }
@@ -120,6 +124,7 @@ TEST_F(TestRoboteqDriver, BootErrorVendorId)
   roboteqs_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(0x1018, 1, 100000);
   ASSERT_FALSE(TestBoot());
 
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
   roboteqs_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(0x1018, 1, 0);
   ASSERT_TRUE(TestBoot());
 }
@@ -165,7 +170,6 @@ TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
 TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps)
 {
   BootRoboteqDriver();
-  std::this_thread::sleep_for(std::chrono::milliseconds(150));
 
   panther_hardware_interfaces::MotorDriverState fb_motor_1 =
     roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadMotorDriverState();
@@ -243,7 +247,7 @@ TEST_F(TestRoboteqDriver, ReadDriverState)
 
 TEST_F(TestRoboteqDriver, ReadDriverStateTimestamp)
 {
-  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  BootRoboteqDriver();
 
   panther_hardware_interfaces::DriverState fb1 = roboteq_driver_->ReadDriverState();
 
@@ -295,35 +299,62 @@ TEST_F(TestRoboteqDriver, SendRoboteqCmd)
 
 TEST_F(TestRoboteqDriver, ResetRoboteqScript)
 {
-  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  BootRoboteqDriver();
   roboteqs_mock_->GetDriver()->SetResetRoboteqScript(65);
   roboteq_driver_->ResetScript();
 
   EXPECT_EQ(roboteqs_mock_->GetDriver()->GetResetRoboteqScript(), 2);
 }
 
+TEST_F(TestRoboteqDriver, ResetRoboteqScriptSDOTimeoutReset)
+{
+  BootRoboteqDriver();
+
+  roboteqs_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 100000);
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { roboteq_driver_->ResetScript(); }, "SDO protocol timed out"));
+
+  roboteqs_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 0);
+  ASSERT_NO_THROW(roboteq_driver_->ResetScript());
+}
+
 TEST_F(TestRoboteqDriver, ReadRoboteqTurnOnEStop)
 {
-  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  BootRoboteqDriver();
   roboteqs_mock_->GetDriver()->SetTurnOnEStop(65);
   roboteq_driver_->TurnOnEStop();
 
   EXPECT_EQ(roboteqs_mock_->GetDriver()->GetTurnOnEStop(), 1);
 }
 
+TEST_F(TestRoboteqDriver, TurnOnEStopTimeout)
+{
+  BootRoboteqDriver();
+  roboteqs_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200C, 0, 100000);
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { roboteq_driver_->TurnOnEStop(); }, "SDO protocol timed out"));
+}
+
 TEST_F(TestRoboteqDriver, TurnOffEStop)
 {
-  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  BootRoboteqDriver();
   roboteqs_mock_->GetDriver()->SetTurnOffEStop(65);
   roboteq_driver_->TurnOffEStop();
 
   EXPECT_EQ(roboteqs_mock_->GetDriver()->GetTurnOffEStop(), 1);
 }
 
+TEST_F(TestRoboteqDriver, TurnOffEStopTimeout)
+{
+  BootRoboteqDriver();
+  roboteqs_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200D, 0, 100000);
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { roboteq_driver_->TurnOffEStop(); }, "SDO protocol timed out"));
+}
+
 TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel1)
 {
   BootRoboteqDriver();
-  std::this_thread::sleep_for(std::chrono::milliseconds(150));
   roboteqs_mock_->GetDriver()->SetTurnOnSafetyStop(67);
   roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop();
 
@@ -333,7 +364,6 @@ TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel1)
 TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel2)
 {
   BootRoboteqDriver();
-  std::this_thread::sleep_for(std::chrono::milliseconds(150));
   roboteqs_mock_->GetDriver()->SetTurnOnSafetyStop(65);
   roboteq_driver_->GetMotorDriver(kMotor2Name)->TurnOnSafetyStop();
 
@@ -342,14 +372,15 @@ TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel2)
 
 TEST_F(TestRoboteqDriver, WriteTimeout)
 {
-  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  BootRoboteqDriver();
   roboteqs_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 200000);
-  ASSERT_THROW(
-    roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop(), std::runtime_error);
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop(); },
+    "SDO protocol timed out"));
 }
 
-// OnCanError isn't tested, because it reacts to lower-level CAN errors (CRC), which are hard to
-// simulate, but it would be nice to add it
+// OnCanError/OnHeartbeatTimeout isn't tested, because it reacts to lower-level CAN errors (CRC),
+// which are hard to simulate, but it would be nice to add it
 
 int main(int argc, char ** argv)
 {
diff --git a/panther_hardware_interfaces/test/utils/roboteqs_mock.hpp b/panther_hardware_interfaces/test/utils/roboteqs_mock.hpp
index 14fa4426..98edd777 100644
--- a/panther_hardware_interfaces/test/utils/roboteqs_mock.hpp
+++ b/panther_hardware_interfaces/test/utils/roboteqs_mock.hpp
@@ -267,7 +267,7 @@ class RoboteqsMock
   ~RoboteqsMock() {}
 
   /**
-   * @brief Starts CAN communication and creates two simulated Roboteqs, that publish PDOs with set
+   * @brief Starts CAN communication and creates a simulated Roboteq, that publish PDOs with set
    * frequencies
    *
    * @param motors_states_period period of motors states publishing thread

From 412b810995a7e7d2fad83702c1ad0f4eef3ebe6c Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Fri, 23 Aug 2024 09:42:28 +0000
Subject: [PATCH 017/100] update panther_msgs version

---
 panther/panther_hardware.repos   | 2 +-
 panther/panther_simulation.repos | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/panther/panther_hardware.repos b/panther/panther_hardware.repos
index 27289d3b..171ec29f 100644
--- a/panther/panther_hardware.repos
+++ b/panther/panther_hardware.repos
@@ -10,7 +10,7 @@ repositories:
   panther_msgs:
     type: git
     url: https://github.com/husarion/panther_msgs.git
-    version: fcee4d9f249a62adc113eb80be4885b08024ee9c
+    version: ros2-lynx-devel
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git
diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos
index b3e2233f..00ce5283 100644
--- a/panther/panther_simulation.repos
+++ b/panther/panther_simulation.repos
@@ -10,7 +10,7 @@ repositories:
   panther_msgs:
     type: git
     url: https://github.com/husarion/panther_msgs.git
-    version: fcee4d9f249a62adc113eb80be4885b08024ee9c
+    version: ros2-lynx-devel
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git

From 2ed9521e68cdfb65ba6cb15dafb9530dd13f052e Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Mon, 26 Aug 2024 09:55:06 +0000
Subject: [PATCH 018/100] rename motors_controller to robot_driver

---
 panther_hardware_interfaces/CMakeLists.txt    |  51 +-
 .../canopen_manager.hpp                       |   6 +-
 .../driver.hpp                                |   6 +-
 .../robot_driver.hpp}                         | 139 ++++-
 .../roboteq_data_converters.hpp               |   8 +-
 .../roboteq_driver.hpp                        |   8 +-
 .../roboteq_error_filter.hpp                  |   6 +-
 .../canopen_manager.cpp                       |   2 +-
 .../motors_controller.cpp                     |  62 +--
 .../roboteq_data_converters.cpp               |   2 +-
 .../roboteq_driver.cpp                        |   2 +-
 .../roboteq_error_filter.cpp                  |   2 +-
 .../test_motors_controller.cpp                | 501 ------------------
 .../test_canopen_manager.cpp                  |   2 +-
 .../test_roboteq_driver.cpp                   |   4 +-
 .../test/utils/test_constants.hpp             |   4 +-
 16 files changed, 202 insertions(+), 603 deletions(-)
 rename panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/{motors_controller => robot_driver}/canopen_manager.hpp (92%)
 rename panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/{motors_controller => robot_driver}/driver.hpp (93%)
 rename panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/{motors_controller/motors_controller.hpp => robot_driver/robot_driver.hpp} (51%)
 rename panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/{motors_controller => robot_driver}/roboteq_data_converters.hpp (94%)
 rename panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/{motors_controller => robot_driver}/roboteq_driver.hpp (94%)
 rename panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/{motors_controller => robot_driver}/roboteq_error_filter.hpp (91%)
 rename panther_hardware_interfaces/src/panther_system/{motors_controller => robot_driver}/canopen_manager.cpp (98%)
 rename panther_hardware_interfaces/src/panther_system/{motors_controller => robot_driver}/motors_controller.cpp (74%)
 rename panther_hardware_interfaces/src/panther_system/{motors_controller => robot_driver}/roboteq_data_converters.cpp (98%)
 rename panther_hardware_interfaces/src/panther_system/{motors_controller => robot_driver}/roboteq_driver.cpp (99%)
 rename panther_hardware_interfaces/src/panther_system/{motors_controller => robot_driver}/roboteq_error_filter.cpp (96%)
 delete mode 100644 panther_hardware_interfaces/test/panther_system/motors_controller/test_motors_controller.cpp
 rename panther_hardware_interfaces/test/unit/panther_system/{motors_controller => robot_driver}/test_canopen_manager.cpp (96%)
 rename panther_hardware_interfaces/test/unit/panther_system/{motors_controller => robot_driver}/test_roboteq_driver.cpp (98%)

diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt
index b87fda02..fd997127 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/panther_hardware_interfaces/CMakeLists.txt
@@ -55,11 +55,11 @@ add_library(
   src/panther_imu_sensor/panther_imu_sensor.cpp
   src/panther_system/gpio/gpio_controller.cpp
   src/panther_system/gpio/gpio_driver.cpp
-  src/panther_system/motors_controller/canopen_manager.cpp
-  src/panther_system/motors_controller/motors_controller.cpp
-  src/panther_system/motors_controller/roboteq_data_converters.cpp
-  src/panther_system/motors_controller/roboteq_driver.cpp
-  src/panther_system/motors_controller/roboteq_error_filter.cpp
+  src/panther_system/robot_driver/canopen_manager.cpp
+  src/panther_system/robot_driver/robot_driver.cpp
+  src/panther_system/robot_driver/roboteq_data_converters.cpp
+  src/panther_system/robot_driver/roboteq_driver.cpp
+  src/panther_system/robot_driver/roboteq_error_filter.cpp
   src/panther_system/panther_system_e_stop.cpp
   src/panther_system/panther_system_ros_interface.cpp
   src/panther_system/panther_system.cpp
@@ -100,14 +100,13 @@ if(BUILD_TESTING)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_error_filter
-    test/panther_system/motors_controller/test_roboteq_error_filter.cpp
-    src/panther_system/motors_controller/roboteq_error_filter.cpp)
+    test/panther_system/robot_driver/test_roboteq_error_filter.cpp
+    src/panther_system/robot_driver/roboteq_error_filter.cpp)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_data_converters
-    test/panther_system/motors_controller/test_roboteq_data_converters.cpp
-    src/panther_system/motors_controller/roboteq_data_converters.cpp
-    src/utils.cpp)
+    test/panther_system/robot_driver/test_roboteq_data_converters.cpp
+    src/panther_system/robot_driver/roboteq_data_converters.cpp src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_roboteq_data_converters
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
@@ -119,9 +118,9 @@ if(BUILD_TESTING)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_canopen_manager
-    test/unit/panther_system/motors_controller/test_canopen_manager.cpp
-    src/panther_system/motors_controller/canopen_manager.cpp
-    src/panther_system/motors_controller/roboteq_driver.cpp
+    test/unit/panther_system/robot_driver/test_canopen_manager.cpp
+    src/panther_system/robot_driver/canopen_manager.cpp
+    src/panther_system/robot_driver/roboteq_driver.cpp
     src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_canopen_manager
@@ -134,9 +133,9 @@ if(BUILD_TESTING)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_driver
-    test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
-    src/panther_system/motors_controller/canopen_manager.cpp
-    src/panther_system/motors_controller/roboteq_driver.cpp
+    test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
+    src/panther_system/robot_driver/canopen_manager.cpp
+    src/panther_system/robot_driver/roboteq_driver.cpp
     src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_roboteq_driver
@@ -148,20 +147,20 @@ if(BUILD_TESTING)
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gtest(
-    ${PROJECT_NAME}_test_motors_controller
-    test/panther_system/motors_controller/test_motors_controller.cpp
-    src/panther_system/motors_controller/canopen_manager.cpp
-    src/panther_system/motors_controller/roboteq_driver.cpp
-    src/panther_system/motors_controller/roboteq_data_converters.cpp
-    src/panther_system/motors_controller/motors_controller.cpp
+    ${PROJECT_NAME}_test_robot_driver
+    test/panther_system/robot_driver/test_robot_driver.cpp
+    src/panther_system/robot_driver/canopen_manager.cpp
+    src/panther_system/robot_driver/roboteq_driver.cpp
+    src/panther_system/robot_driver/roboteq_data_converters.cpp
+    src/panther_system/robot_driver/robot_driver.cpp
     src/utils.cpp)
   target_include_directories(
-    ${PROJECT_NAME}_test_motors_controller
+    ${PROJECT_NAME}_test_robot_driver
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
-  ament_target_dependencies(${PROJECT_NAME}_test_motors_controller rclcpp
+  ament_target_dependencies(${PROJECT_NAME}_test_robot_driver rclcpp
                             panther_msgs panther_utils)
-  target_link_libraries(${PROJECT_NAME}_test_motors_controller
+  target_link_libraries(${PROJECT_NAME}_test_robot_driver
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver
@@ -183,7 +182,7 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_panther_system_ros_interface
     test/panther_system/test_panther_system_ros_interface.cpp
     src/panther_system/panther_system_ros_interface.cpp
-    src/panther_system/motors_controller/roboteq_data_converters.cpp
+    src/panther_system/robot_driver/roboteq_data_converters.cpp
     src/utils.cpp
     src/panther_system/gpio/gpio_controller.cpp
     src/panther_system/gpio/gpio_driver.cpp)
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp
similarity index 92%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp
rename to panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp
index fb9721c7..c58a3107 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_MANAGER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_MANAGER_HPP_
+#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_CANOPEN_MANAGER_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_CANOPEN_MANAGER_HPP_
 
 #include <atomic>
 #include <chrono>
@@ -110,4 +110,4 @@ class CANopenManager
 
 }  // namespace panther_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_MANAGER_HPP_
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_CANOPEN_MANAGER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp
similarity index 93%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/driver.hpp
rename to panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp
index 87c341d0..9a62e237 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_DRIVER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_DRIVER_HPP_
+#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_DRIVER_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_DRIVER_HPP_
 
 #include <atomic>
 #include <chrono>
@@ -147,4 +147,4 @@ class Driver
 
 }  // namespace panther_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_DRIVER_HPP_
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
similarity index 51%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp
rename to panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
index e8e7c00a..f8378155 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
@@ -12,44 +12,146 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_MOTORS_CONTROLLER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_MOTORS_CONTROLLER_HPP_
+#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_
 
 #include <chrono>
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp"
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp"
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
 
 namespace panther_hardware_interfaces
 {
 
+/**
+ * @brief Abstract class for managing robot drivers.
+ */
+class RobotDriver
+{
+public:
+  RobotDriver() = default;
+
+  ~RobotDriver() { Deinitialize(); }
+
+  /**
+   * @brief Initialize robot driver
+   *
+   * @exception std::runtime_error if boot fails
+   */
+  virtual void Initialize() = 0;
+
+  /**
+   * @brief Deinitialize robot driver
+   */
+  virtual void Deinitialize() = 0;
+
+  /**
+   * @brief Activate procedure for the driver
+   *
+   * @exception std::runtime_error if any procedure step fails
+   */
+  virtual void Activate() = 0;
+
+  /**
+   * @brief Updates current communication state with the drivers
+   *
+   * @exception std::runtime_error if error was detected
+   */
+  virtual void UpdateCommunicationState() = 0;
+
+  /**
+   * @brief Updates current motors' state (position, velocity, current).
+   *
+   * @exception std::runtime_error if error was detected
+   */
+  virtual void UpdateMotorsState() = 0;
+
+  /**
+   * @brief Updates current driver state (flags, temperatures, voltage, battery current)
+   *
+   * @exception std::runtime_error if error was detected
+   */
+  virtual void UpdateDriversState() = 0;
+
+  /**
+   * @brief Get data feedback from the driver
+   *
+   * @param name name of the data to get
+   *
+   * @return data feedback
+   * @exception std::runtime_error if data with the given name does not exist
+   */
+  virtual const RoboteqData & GetData(const std::string & name) = 0;
+
+  /**
+   * @brief Write speed commands to motors
+   *
+   * @param speed_fl front left motor speed in rad/s
+   * @param speed_fr front right motor speed in rad/s
+   * @param speed_rl rear left motor speed in rad/s
+   * @param speed_rr rear right motor speed in rad/s
+   *
+   * @exception std::runtime_error if send command fails
+   */
+  virtual void SendSpeedCommands(
+    const float speed_fl, const float speed_fr, const float speed_rl, const float speed_rr) = 0;
+
+  /**
+   * @brief Turns on E-stop
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void TurnOnEStop() = 0;
+
+  /**
+   * @brief Turns off E-stop
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void TurnOffEStop() = 0;
+
+  /**
+   * @brief Turns on safety stop. To turn it off, it is necessary to send
+   * 0 commands to motors.
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void TurnOnSafetyStop() = 0;
+
+  /**
+   * @brief Attempt to clear driver error flags by sending 0 velocity commands to motors. If driver
+   * faults still exist, the error flag will remain active.
+   */
+  inline void AttemptErrorFlagResetWithZeroSpeed() { SendSpeedCommands(0.0, 0.0, 0.0, 0.0); };
+};
+
 /**
  * @brief This class abstracts the usage of two Roboteq controllers.
- * It uses canopen_controller for communication with Roboteq controllers,
+ * It uses canopen_manager for communication with Roboteq controllers,
  * implements the activation procedure for controllers (resets script and sends initial 0 command),
  * and provides methods to get data feedback and send commands.
  * Data is converted between raw Roboteq formats and SI units using roboteq_data_converters.
  */
-class MotorsController
+class PantherRobotDriver : public RobotDriver
 {
 public:
-  MotorsController(
+  PantherRobotDriver(
     const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings);
 
-  ~MotorsController() { Deinitialize(); }
+  ~PantherRobotDriver() = default;
 
   /**
    * @brief Starts CAN communication and waits for boot to finish
    *
    * @exception std::runtime_error if boot fails
    */
-  void Initialize();
+  void Initialize() override;
 
   /**
    * @brief Deinitialize can communication
    */
-  void Deinitialize();
+  void Deinitialize() override;
 
   /**
    * @brief Activate procedure for Roboteq drivers - reset scripts and send 0 commands on both
@@ -57,31 +159,30 @@ class MotorsController
    *
    * @exception std::runtime_error if any procedure step fails
    */
-  void Activate();
+  void Activate() override;
 
   /**
    * @brief Updates current communication state with Roboteq drivers
    *
    * @exception std::runtime_error if CAN error was detected
    */
-  void UpdateCommunicationState();
+  void UpdateCommunicationState() override;
 
   /**
    * @brief Updates current motors' state (position, velocity, current).
    *
    * @exception std::runtime_error if CAN error was detected
    */
-  void UpdateMotorsState();
+  void UpdateMotorsState() override;
 
   /**
    * @brief Updates current Roboteq driver state (flags, temperatures, voltage, battery current)
    *
    * @exception std::runtime_error if CAN error was detected
    */
-  void UpdateDriversState();
+  void UpdateDriversState() override;
 
-  const RoboteqData & GetFrontData() { return front_data_; }
-  const RoboteqData & GetRearData() { return rear_data_; }
+  const RoboteqData & GetData(const std::string & name) override;
 
   /**
    * @brief Write speed commands to motors
@@ -132,7 +233,7 @@ class MotorsController
 
   bool initialized_ = false;
 
-  CANopenController canopen_controller_;
+  CANopenManager canopen_manager_;
 
   RoboteqData front_data_;
   RoboteqData rear_data_;
@@ -145,4 +246,4 @@ class MotorsController
 
 }  // namespace panther_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_MOTORS_CONTROLLER_HPP_
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp
similarity index 94%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp
rename to panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp
index 4a6d6b25..cfb897c5 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_DATA_CONVERTERS_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_DATA_CONVERTERS_HPP_
+#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DATA_CONVERTERS_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DATA_CONVERTERS_HPP_
 
 #include <algorithm>
 #include <bitset>
@@ -25,7 +25,7 @@
 #include "panther_msgs/msg/runtime_error.hpp"
 #include "panther_msgs/msg/script_flag.hpp"
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
 #include "panther_hardware_interfaces/utils.hpp"
 
 namespace panther_hardware_interfaces
@@ -246,4 +246,4 @@ class RoboteqData
 
 }  // namespace panther_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_DATA_CONVERTERS_HPP_
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DATA_CONVERTERS_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp
similarity index 94%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
rename to panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp
index 05859405..ee9815c2 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_DRIVER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_DRIVER_HPP_
+#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DRIVER_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DRIVER_HPP_
 
 #include <atomic>
 #include <chrono>
@@ -25,7 +25,7 @@
 
 #include "lely/coapp/loop_driver.hpp"
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/driver.hpp"
 
 namespace panther_hardware_interfaces
 {
@@ -196,4 +196,4 @@ class RoboteqMotorDriver : public MotorDriver
 
 }  // namespace panther_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_DRIVER_HPP_
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp
similarity index 91%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp
rename to panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp
index ceadba30..53a13d10 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_ERROR_FILTER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_ERROR_FILTER_HPP_
+#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ERROR_FILTER_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ERROR_FILTER_HPP_
 
 #include <atomic>
 #include <map>
@@ -104,4 +104,4 @@ class RoboteqErrorFilter
 
 }  // namespace panther_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_ERROR_FILTER_HPP_
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ERROR_FILTER_HPP_
diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_manager.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/canopen_manager.cpp
similarity index 98%
rename from panther_hardware_interfaces/src/panther_system/motors_controller/canopen_manager.cpp
rename to panther_hardware_interfaces/src/panther_system/robot_driver/canopen_manager.cpp
index 82573100..52c7b1c3 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_manager.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/canopen_manager.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
 
 #include <condition_variable>
 #include <filesystem>
diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/motors_controller.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/motors_controller.cpp
similarity index 74%
rename from panther_hardware_interfaces/src/panther_system/motors_controller/motors_controller.cpp
rename to panther_hardware_interfaces/src/panther_system/robot_driver/motors_controller.cpp
index e260fa86..4c366606 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/motors_controller.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/motors_controller.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
 
 #include <chrono>
 #include <ctime>
@@ -21,16 +21,16 @@
 
 #include "lely/util/chrono.hpp"
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp"
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp"
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
 
 namespace panther_hardware_interfaces
 {
 
 MotorsController::MotorsController(
   const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings)
-: canopen_controller_(canopen_settings),
+: canopen_manager_(canopen_settings),
   front_data_(drivetrain_settings),
   rear_data_(drivetrain_settings),
   roboteq_vel_cmd_converter_(drivetrain_settings),
@@ -46,7 +46,7 @@ void MotorsController::Initialize()
   }
 
   try {
-    canopen_controller_.Initialize();
+    canopen_manager_.Initialize();
   } catch (const std::runtime_error & e) {
     throw e;
   }
@@ -56,7 +56,7 @@ void MotorsController::Initialize()
 
 void MotorsController::Deinitialize()
 {
-  canopen_controller_.Deinitialize();
+  canopen_manager_.Deinitialize();
   initialized_ = false;
 }
 
@@ -66,14 +66,14 @@ void MotorsController::Activate()
   // and then send 0 commands for some time (also 1 second)
 
   try {
-    canopen_controller_.GetFrontDriver()->ResetRoboteqScript();
+    canopen_manager_.GetFrontDriver()->ResetRoboteqScript();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Front driver reset Roboteq script exception: " + std::string(e.what()));
   }
 
   try {
-    canopen_controller_.GetRearDriver()->ResetRoboteqScript();
+    canopen_manager_.GetRearDriver()->ResetRoboteqScript();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Rear driver reset Roboteq script exception: " + std::string(e.what()));
@@ -82,13 +82,13 @@ void MotorsController::Activate()
   std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 
   try {
-    canopen_controller_.GetFrontDriver()->SendRoboteqCmd(0, 0);
+    canopen_manager_.GetFrontDriver()->SendRoboteqCmd(0, 0);
   } catch (const std::runtime_error & e) {
     throw std::runtime_error("Front driver send 0 command exception: " + std::string(e.what()));
   }
 
   try {
-    canopen_controller_.GetRearDriver()->SendRoboteqCmd(0, 0);
+    canopen_manager_.GetRearDriver()->SendRoboteqCmd(0, 0);
   } catch (const std::runtime_error & e) {
     throw std::runtime_error("Rear driver send 0 command exception: " + std::string(e.what()));
   }
@@ -98,11 +98,11 @@ void MotorsController::Activate()
 
 void MotorsController::UpdateCommunicationState()
 {
-  front_data_.SetCANError(canopen_controller_.GetFrontDriver()->IsCANError());
-  rear_data_.SetCANError(canopen_controller_.GetRearDriver()->IsCANError());
+  front_data_.SetCANError(canopen_manager_.GetFrontDriver()->IsCANError());
+  rear_data_.SetCANError(canopen_manager_.GetRearDriver()->IsCANError());
 
-  front_data_.SetHeartbeatTimeout(canopen_controller_.GetFrontDriver()->IsHeartbeatTimeout());
-  rear_data_.SetHeartbeatTimeout(canopen_controller_.GetRearDriver()->IsHeartbeatTimeout());
+  front_data_.SetHeartbeatTimeout(canopen_manager_.GetFrontDriver()->IsHeartbeatTimeout());
+  rear_data_.SetHeartbeatTimeout(canopen_manager_.GetRearDriver()->IsHeartbeatTimeout());
 }
 
 void MotorsController::UpdateMotorsState()
@@ -111,9 +111,9 @@ void MotorsController::UpdateMotorsState()
   clock_gettime(CLOCK_MONOTONIC, &current_time);
 
   SetMotorsStates(
-    front_data_, canopen_controller_.GetFrontDriver()->ReadRoboteqMotorsStates(), current_time);
+    front_data_, canopen_manager_.GetFrontDriver()->ReadRoboteqMotorsStates(), current_time);
   SetMotorsStates(
-    rear_data_, canopen_controller_.GetRearDriver()->ReadRoboteqMotorsStates(), current_time);
+    rear_data_, canopen_manager_.GetRearDriver()->ReadRoboteqMotorsStates(), current_time);
 
   UpdateCommunicationState();
 
@@ -132,9 +132,9 @@ void MotorsController::UpdateDriversState()
   clock_gettime(CLOCK_MONOTONIC, &current_time);
 
   SetDriverState(
-    front_data_, canopen_controller_.GetFrontDriver()->ReadRoboteqDriverState(), current_time);
+    front_data_, canopen_manager_.GetFrontDriver()->ReadRoboteqDriverState(), current_time);
   SetDriverState(
-    rear_data_, canopen_controller_.GetRearDriver()->ReadRoboteqDriverState(), current_time);
+    rear_data_, canopen_manager_.GetRearDriver()->ReadRoboteqDriverState(), current_time);
 
   UpdateCommunicationState();
 
@@ -152,23 +152,23 @@ void MotorsController::SendSpeedCommands(
 {
   // Channel 1 - right motor, Channel 2 - left motor
   try {
-    canopen_controller_.GetFrontDriver()->SendRoboteqCmd(
+    canopen_manager_.GetFrontDriver()->SendRoboteqCmd(
       roboteq_vel_cmd_converter_.Convert(speed_fr), roboteq_vel_cmd_converter_.Convert(speed_fl));
   } catch (const std::runtime_error & e) {
     throw std::runtime_error("Front driver send Roboteq cmd failed: " + std::string(e.what()));
   }
   try {
-    canopen_controller_.GetRearDriver()->SendRoboteqCmd(
+    canopen_manager_.GetRearDriver()->SendRoboteqCmd(
       roboteq_vel_cmd_converter_.Convert(speed_rr), roboteq_vel_cmd_converter_.Convert(speed_rl));
   } catch (const std::runtime_error & e) {
     throw std::runtime_error("Rear driver send Roboteq cmd failed: " + std::string(e.what()));
   }
 
-  if (canopen_controller_.GetFrontDriver()->IsCANError()) {
+  if (canopen_manager_.GetFrontDriver()->IsCANError()) {
     throw std::runtime_error(
       "CAN error detected on the front driver when trying to write speed commands.");
   }
-  if (canopen_controller_.GetRearDriver()->IsCANError()) {
+  if (canopen_manager_.GetRearDriver()->IsCANError()) {
     throw std::runtime_error(
       "CAN error detected on the rear driver when trying to write speed commands.");
   }
@@ -177,13 +177,13 @@ void MotorsController::SendSpeedCommands(
 void MotorsController::TurnOnEStop()
 {
   try {
-    canopen_controller_.GetFrontDriver()->TurnOnEStop();
+    canopen_manager_.GetFrontDriver()->TurnOnEStop();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Failed to turn on E-stop on the front driver: " + std::string(e.what()));
   }
   try {
-    canopen_controller_.GetRearDriver()->TurnOnEStop();
+    canopen_manager_.GetRearDriver()->TurnOnEStop();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Failed to turn on E-stop on the rear driver: " + std::string(e.what()));
@@ -193,13 +193,13 @@ void MotorsController::TurnOnEStop()
 void MotorsController::TurnOffEStop()
 {
   try {
-    canopen_controller_.GetFrontDriver()->TurnOffEStop();
+    canopen_manager_.GetFrontDriver()->TurnOffEStop();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Failed to turn off E-stop on the front driver: " + std::string(e.what()));
   }
   try {
-    canopen_controller_.GetRearDriver()->TurnOffEStop();
+    canopen_manager_.GetRearDriver()->TurnOffEStop();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Failed to turn off E-stop on the rear driver: " + std::string(e.what()));
@@ -209,15 +209,15 @@ void MotorsController::TurnOffEStop()
 void MotorsController::TurnOnSafetyStop()
 {
   try {
-    canopen_controller_.GetFrontDriver()->TurnOnSafetyStopChannel1();
-    canopen_controller_.GetFrontDriver()->TurnOnSafetyStopChannel2();
+    canopen_manager_.GetFrontDriver()->TurnOnSafetyStopChannel1();
+    canopen_manager_.GetFrontDriver()->TurnOnSafetyStopChannel2();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Failed to turn on safety stop on the front driver: " + std::string(e.what()));
   }
   try {
-    canopen_controller_.GetRearDriver()->TurnOnSafetyStopChannel1();
-    canopen_controller_.GetRearDriver()->TurnOnSafetyStopChannel2();
+    canopen_manager_.GetRearDriver()->TurnOnSafetyStopChannel1();
+    canopen_manager_.GetRearDriver()->TurnOnSafetyStopChannel2();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Failed to turn on safety stop on the rear driver: " + std::string(e.what()));
diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_data_converters.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp
similarity index 98%
rename from panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_data_converters.cpp
rename to panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp
index 4c877913..a025a3f1 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_data_converters.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
 
 #include <cmath>
 
diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp
similarity index 99%
rename from panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp
rename to panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp
index d563a495..c2c8d853 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
 
 #include <chrono>
 #include <cmath>
diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_error_filter.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_error_filter.cpp
similarity index 96%
rename from panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_error_filter.cpp
rename to panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_error_filter.cpp
index c0092af9..1d15abeb 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_error_filter.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_error_filter.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp"
 
 #include <algorithm>
 #include <atomic>
diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_motors_controller.cpp b/panther_hardware_interfaces/test/panther_system/motors_controller/test_motors_controller.cpp
deleted file mode 100644
index 789415a8..00000000
--- a/panther_hardware_interfaces/test/panther_system/motors_controller/test_motors_controller.cpp
+++ /dev/null
@@ -1,501 +0,0 @@
-// Copyright 2023 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 <chrono>
-#include <cstdint>
-#include <memory>
-#include <stdexcept>
-#include <string>
-#include <thread>
-
-#include <gtest/gtest.h>
-
-#include <panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp>
-#include <panther_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp>
-#include <panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp>
-
-#include "utils/roboteqs_mock.hpp"
-#include "utils/test_constants.hpp"
-
-class TestMotorsControllerInitialization : public ::testing::Test
-{
-public:
-  TestMotorsControllerInitialization()
-  {
-    motors_controller_ = std::make_unique<panther_hardware_interfaces::MotorsController>(
-      panther_hardware_interfaces_test::kCANopenSettings,
-      panther_hardware_interfaces_test::kDrivetrainSettings);
-
-    roboteqs_mock_ = std::make_shared<panther_hardware_interfaces_test::RoboteqsMock>();
-    roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
-  }
-
-  ~TestMotorsControllerInitialization()
-  {
-    roboteqs_mock_->Stop();
-    roboteqs_mock_.reset();
-  }
-
-  std::shared_ptr<panther_hardware_interfaces_test::RoboteqsMock> roboteqs_mock_;
-  std::unique_ptr<panther_hardware_interfaces::MotorsController> motors_controller_;
-};
-
-// These tests are related to canopen_controller tests, where boot should be already tested
-
-TEST_F(TestMotorsControllerInitialization, Initialize)
-{
-  ASSERT_NO_THROW(motors_controller_->Initialize());
-  ASSERT_NO_THROW(motors_controller_->Deinitialize());
-
-  // Check if deinitialization worked correctly - initialize once again
-  ASSERT_NO_THROW(motors_controller_->Initialize());
-  ASSERT_NO_THROW(motors_controller_->Deinitialize());
-}
-
-TEST_F(TestMotorsControllerInitialization, ErrorDeviceType)
-{
-  roboteqs_mock_->GetFrontDriver()->SetOnReadWait<std::uint32_t>(0x1000, 0, 100000);
-  ASSERT_THROW(motors_controller_->Initialize(), std::runtime_error);
-  ASSERT_NO_THROW(motors_controller_->Deinitialize());
-
-  roboteqs_mock_->GetFrontDriver()->SetOnReadWait<std::uint32_t>(0x1000, 0, 0);
-  ASSERT_NO_THROW(motors_controller_->Initialize());
-  ASSERT_NO_THROW(motors_controller_->Deinitialize());
-}
-
-TEST_F(TestMotorsControllerInitialization, ErrorVendorId)
-{
-  roboteqs_mock_->GetRearDriver()->SetOnReadWait<std::uint32_t>(0x1018, 1, 100000);
-  ASSERT_THROW(motors_controller_->Initialize(), std::runtime_error);
-  ASSERT_NO_THROW(motors_controller_->Deinitialize());
-
-  roboteqs_mock_->GetRearDriver()->SetOnReadWait<std::uint32_t>(0x1018, 1, 0);
-  ASSERT_NO_THROW(motors_controller_->Initialize());
-  ASSERT_NO_THROW(motors_controller_->Deinitialize());
-}
-
-TEST_F(TestMotorsControllerInitialization, Activate)
-{
-  using panther_hardware_interfaces_test::DriverChannel;
-
-  ASSERT_NO_THROW(motors_controller_->Initialize());
-
-  roboteqs_mock_->GetFrontDriver()->SetRoboteqCmd(DriverChannel::CHANNEL1, 234);
-  roboteqs_mock_->GetFrontDriver()->SetRoboteqCmd(DriverChannel::CHANNEL2, 32);
-  roboteqs_mock_->GetRearDriver()->SetRoboteqCmd(DriverChannel::CHANNEL1, 54);
-  roboteqs_mock_->GetRearDriver()->SetRoboteqCmd(DriverChannel::CHANNEL2, 12);
-
-  roboteqs_mock_->GetFrontDriver()->SetResetRoboteqScript(65);
-  roboteqs_mock_->GetRearDriver()->SetResetRoboteqScript(23);
-
-  ASSERT_NO_THROW(motors_controller_->Activate());
-
-  ASSERT_EQ(roboteqs_mock_->GetFrontDriver()->GetResetRoboteqScript(), 2);
-  ASSERT_EQ(roboteqs_mock_->GetRearDriver()->GetResetRoboteqScript(), 2);
-
-  ASSERT_EQ(roboteqs_mock_->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0);
-  ASSERT_EQ(roboteqs_mock_->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0);
-  ASSERT_EQ(roboteqs_mock_->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0);
-  ASSERT_EQ(roboteqs_mock_->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0);
-
-  motors_controller_->Deinitialize();
-}
-
-TEST_F(TestMotorsControllerInitialization, ActivateSDOTimeoutReset)
-{
-  ASSERT_NO_THROW(motors_controller_->Initialize());
-  roboteqs_mock_->GetFrontDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 100000);
-  ASSERT_THROW(motors_controller_->Activate(), std::runtime_error);
-  ASSERT_NO_THROW(motors_controller_->Deinitialize());
-}
-
-class TestMotorsController : public TestMotorsControllerInitialization
-{
-public:
-  TestMotorsController()
-  {
-    motors_controller_->Initialize();
-    motors_controller_->Activate();
-  }
-
-  ~TestMotorsController() { motors_controller_->Deinitialize(); }
-};
-
-TEST_F(TestMotorsController, UpdateMotorsState)
-{
-  using panther_hardware_interfaces_test::DriverChannel;
-
-  using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
-  using panther_hardware_interfaces_test::kRbtqPosFbToRad;
-  using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
-
-  const std::int32_t fl_pos = 101;
-  const std::int32_t fl_vel = 102;
-  const std::int32_t fl_current = 103;
-  const std::int32_t fr_pos = 201;
-  const std::int32_t fr_vel = 202;
-  const std::int32_t fr_current = 203;
-  const std::int32_t rl_pos = 301;
-  const std::int32_t rl_vel = 302;
-  const std::int32_t rl_current = 303;
-  const std::int32_t rr_pos = 401;
-  const std::int32_t rr_vel = 402;
-  const std::int32_t rr_current = 403;
-
-  roboteqs_mock_->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL2, fl_pos);
-  roboteqs_mock_->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL1, fr_pos);
-  roboteqs_mock_->GetRearDriver()->SetPosition(DriverChannel::CHANNEL2, rl_pos);
-  roboteqs_mock_->GetRearDriver()->SetPosition(DriverChannel::CHANNEL1, rr_pos);
-
-  roboteqs_mock_->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL2, fl_vel);
-  roboteqs_mock_->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL1, fr_vel);
-  roboteqs_mock_->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL2, rl_vel);
-  roboteqs_mock_->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL1, rr_vel);
-
-  roboteqs_mock_->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL2, fl_current);
-  roboteqs_mock_->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL1, fr_current);
-  roboteqs_mock_->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL2, rl_current);
-  roboteqs_mock_->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL1, rr_current);
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-
-  motors_controller_->UpdateMotorsState();
-
-  const auto & fl = motors_controller_->GetFrontData().GetLeftMotorState();
-  const auto & fr = motors_controller_->GetFrontData().GetRightMotorState();
-  const auto & rl = motors_controller_->GetRearData().GetLeftMotorState();
-  const auto & rr = motors_controller_->GetRearData().GetRightMotorState();
-
-  EXPECT_FLOAT_EQ(fl.GetPosition(), fl_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(fl.GetVelocity(), fl_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(fl.GetTorque(), fl_current * kRbtqCurrentFbToNewtonMeters);
-
-  EXPECT_FLOAT_EQ(fr.GetPosition(), fr_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(fr.GetVelocity(), fr_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(fr.GetTorque(), fr_current * kRbtqCurrentFbToNewtonMeters);
-
-  EXPECT_FLOAT_EQ(rl.GetPosition(), rl_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(rl.GetVelocity(), rl_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(rl.GetTorque(), rl_current * kRbtqCurrentFbToNewtonMeters);
-
-  EXPECT_FLOAT_EQ(rr.GetPosition(), rr_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(rr.GetVelocity(), rr_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(rr.GetTorque(), rr_current * kRbtqCurrentFbToNewtonMeters);
-}
-
-TEST_F(TestMotorsController, UpdateMotorsStateTimestamps)
-{
-  motors_controller_->UpdateMotorsState();
-
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
-    std::chrono::milliseconds(10));
-
-  motors_controller_->UpdateMotorsState();
-
-  EXPECT_FALSE(motors_controller_->GetFrontData().IsMotorStatesDataTimedOut());
-  EXPECT_FALSE(motors_controller_->GetRearData().IsMotorStatesDataTimedOut());
-}
-
-TEST(TestMotorsControllerOthers, UpdateMotorsStateTimeout)
-{
-  std::shared_ptr<panther_hardware_interfaces_test::RoboteqsMock> roboteqs_mock_;
-  std::unique_ptr<panther_hardware_interfaces::MotorsController> motors_controller_;
-
-  motors_controller_ = std::make_unique<panther_hardware_interfaces::MotorsController>(
-    panther_hardware_interfaces_test::kCANopenSettings,
-    panther_hardware_interfaces_test::kDrivetrainSettings);
-
-  roboteqs_mock_ = std::make_shared<panther_hardware_interfaces_test::RoboteqsMock>();
-
-  roboteqs_mock_->Start(std::chrono::milliseconds(200), std::chrono::milliseconds(50));
-
-  motors_controller_->Initialize();
-  motors_controller_->Activate();
-
-  motors_controller_->UpdateMotorsState();
-
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
-    std::chrono::milliseconds(10));
-
-  motors_controller_->UpdateMotorsState();
-
-  EXPECT_TRUE(motors_controller_->GetFrontData().IsMotorStatesDataTimedOut());
-  EXPECT_TRUE(motors_controller_->GetRearData().IsMotorStatesDataTimedOut());
-  EXPECT_TRUE(motors_controller_->GetFrontData().IsError());
-  EXPECT_TRUE(motors_controller_->GetRearData().IsError());
-
-  motors_controller_->Deinitialize();
-
-  roboteqs_mock_->Stop();
-  roboteqs_mock_.reset();
-}
-
-// Similar to test_roboteq_driver, can_error in update_system_feedback isn't tested, because it
-// reacts to lower-level CAN errors (CRC), which are hard to simulate, but it would be nice to add
-// it
-
-TEST_F(TestMotorsController, UpdateDriverState)
-{
-  using panther_hardware_interfaces_test::DriverChannel;
-  using panther_hardware_interfaces_test::DriverFaultFlags;
-  using panther_hardware_interfaces_test::DriverRuntimeErrors;
-  using panther_hardware_interfaces_test::DriverScriptFlags;
-
-  const std::int16_t f_temp = 30;
-  const std::int16_t r_temp = 32;
-  const std::int16_t f_heatsink_temp = 31;
-  const std::int16_t r_heatsink_temp = 33;
-  const std::uint16_t f_volt = 400;
-  const std::uint16_t r_volt = 430;
-  const std::int16_t f_battery_current_1 = 10;
-  const std::int16_t r_battery_current_1 = 30;
-  const std::int16_t f_battery_current_2 = 30;
-  const std::int16_t r_battery_current_2 = 40;
-
-  roboteqs_mock_->GetFrontDriver()->SetTemperature(f_temp);
-  roboteqs_mock_->GetRearDriver()->SetTemperature(r_temp);
-  roboteqs_mock_->GetFrontDriver()->SetHeatsinkTemperature(f_heatsink_temp);
-  roboteqs_mock_->GetRearDriver()->SetHeatsinkTemperature(r_heatsink_temp);
-  roboteqs_mock_->GetFrontDriver()->SetVoltage(f_volt);
-  roboteqs_mock_->GetRearDriver()->SetVoltage(r_volt);
-  roboteqs_mock_->GetFrontDriver()->SetBatteryCurrent1(f_battery_current_1);
-  roboteqs_mock_->GetRearDriver()->SetBatteryCurrent1(r_battery_current_1);
-  roboteqs_mock_->GetFrontDriver()->SetBatteryCurrent2(f_battery_current_2);
-  roboteqs_mock_->GetRearDriver()->SetBatteryCurrent2(r_battery_current_2);
-
-  roboteqs_mock_->GetFrontDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERHEAT);
-  roboteqs_mock_->GetFrontDriver()->SetDriverScriptFlag(DriverScriptFlags::ENCODER_DISCONNECTED);
-  roboteqs_mock_->GetFrontDriver()->SetDriverRuntimeError(
-    DriverChannel::CHANNEL1, DriverRuntimeErrors::LOOP_ERROR);
-  roboteqs_mock_->GetFrontDriver()->SetDriverRuntimeError(
-    DriverChannel::CHANNEL2, DriverRuntimeErrors::SAFETY_STOP_ACTIVE);
-
-  roboteqs_mock_->GetRearDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERVOLTAGE);
-  roboteqs_mock_->GetRearDriver()->SetDriverScriptFlag(DriverScriptFlags::AMP_LIMITER);
-  roboteqs_mock_->GetRearDriver()->SetDriverRuntimeError(
-    DriverChannel::CHANNEL1, DriverRuntimeErrors::FORWARD_LIMIT_TRIGGERED);
-  roboteqs_mock_->GetRearDriver()->SetDriverRuntimeError(
-    DriverChannel::CHANNEL2, DriverRuntimeErrors::REVERSE_LIMIT_TRIGGERED);
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-
-  motors_controller_->UpdateDriversState();
-
-  const auto & front = motors_controller_->GetFrontData();
-  const auto & rear = motors_controller_->GetRearData();
-  const auto & front_driver_state = motors_controller_->GetFrontData().GetDriverState();
-  const auto & rear_driver_state = motors_controller_->GetRearData().GetDriverState();
-
-  EXPECT_EQ(static_cast<std::int16_t>(front_driver_state.GetTemperature()), f_temp);
-  EXPECT_EQ(
-    static_cast<std::int16_t>(front_driver_state.GetHeatsinkTemperature()), f_heatsink_temp);
-  EXPECT_EQ(static_cast<std::uint16_t>(front_driver_state.GetVoltage() * 10.0), f_volt);
-  EXPECT_EQ(
-    static_cast<std::int16_t>(front_driver_state.GetCurrent() * 10.0),
-    f_battery_current_1 + f_battery_current_2);
-
-  EXPECT_EQ(static_cast<std::int16_t>(rear_driver_state.GetTemperature()), r_temp);
-  EXPECT_EQ(static_cast<std::int16_t>(rear_driver_state.GetHeatsinkTemperature()), r_heatsink_temp);
-  EXPECT_EQ(static_cast<std::uint16_t>(rear_driver_state.GetVoltage() * 10.0), r_volt);
-  EXPECT_EQ(
-    static_cast<std::int16_t>(rear_driver_state.GetCurrent() * 10.0),
-    r_battery_current_1 + r_battery_current_2);
-
-  EXPECT_TRUE(front.GetFaultFlag().GetMessage().overheat);
-  EXPECT_TRUE(front.GetScriptFlag().GetMessage().encoder_disconnected);
-  EXPECT_TRUE(front.GetRightRuntimeError().GetMessage().loop_error);
-  EXPECT_TRUE(front.GetLeftRuntimeError().GetMessage().safety_stop_active);
-
-  EXPECT_TRUE(rear.GetFaultFlag().GetMessage().overvoltage);
-  EXPECT_TRUE(rear.GetScriptFlag().GetMessage().amp_limiter);
-  EXPECT_TRUE(rear.GetRightRuntimeError().GetMessage().forward_limit_triggered);
-  EXPECT_TRUE(rear.GetLeftRuntimeError().GetMessage().reverse_limit_triggered);
-}
-
-TEST_F(TestMotorsController, UpdateDriverStateTimestamps)
-{
-  motors_controller_->UpdateDriversState();
-
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
-    std::chrono::milliseconds(10));
-
-  motors_controller_->UpdateDriversState();
-
-  EXPECT_FALSE(motors_controller_->GetFrontData().IsDriverStateDataTimedOut());
-  EXPECT_FALSE(motors_controller_->GetRearData().IsDriverStateDataTimedOut());
-}
-
-TEST(TestMotorsControllerOthers, UpdateDriverStateTimeout)
-{
-  std::shared_ptr<panther_hardware_interfaces_test::RoboteqsMock> roboteqs_mock_;
-  std::unique_ptr<panther_hardware_interfaces::MotorsController> motors_controller_;
-
-  motors_controller_ = std::make_unique<panther_hardware_interfaces::MotorsController>(
-    panther_hardware_interfaces_test::kCANopenSettings,
-    panther_hardware_interfaces_test::kDrivetrainSettings);
-
-  roboteqs_mock_ = std::make_shared<panther_hardware_interfaces_test::RoboteqsMock>();
-
-  roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(200));
-
-  motors_controller_->Initialize();
-  motors_controller_->Activate();
-
-  motors_controller_->UpdateDriversState();
-
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
-    std::chrono::milliseconds(10));
-
-  motors_controller_->UpdateDriversState();
-
-  EXPECT_TRUE(motors_controller_->GetFrontData().IsDriverStateDataTimedOut());
-  EXPECT_TRUE(motors_controller_->GetRearData().IsDriverStateDataTimedOut());
-  EXPECT_TRUE(motors_controller_->GetFrontData().IsError());
-  EXPECT_TRUE(motors_controller_->GetRearData().IsError());
-
-  motors_controller_->Deinitialize();
-
-  roboteqs_mock_->Stop();
-  roboteqs_mock_.reset();
-}
-
-TEST_F(TestMotorsController, WriteSpeed)
-{
-  using panther_hardware_interfaces_test::DriverChannel;
-  using panther_hardware_interfaces_test::kRadPerSecToRbtqCmd;
-
-  const float fl_v = 0.1;
-  const float fr_v = 0.2;
-  const float rl_v = 0.3;
-  const float rr_v = 0.4;
-
-  ASSERT_NO_THROW(motors_controller_->SendSpeedCommands(fl_v, fr_v, rl_v, rr_v));
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-
-  EXPECT_EQ(
-    roboteqs_mock_->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2),
-    static_cast<std::int32_t>(fl_v * kRadPerSecToRbtqCmd));
-  EXPECT_EQ(
-    roboteqs_mock_->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1),
-    static_cast<std::int32_t>(fr_v * kRadPerSecToRbtqCmd));
-  EXPECT_EQ(
-    roboteqs_mock_->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2),
-    static_cast<std::int32_t>(rl_v * kRadPerSecToRbtqCmd));
-  EXPECT_EQ(
-    roboteqs_mock_->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1),
-    static_cast<std::int32_t>(rr_v * kRadPerSecToRbtqCmd));
-}
-
-// Similar to test_roboteq_driver, can_error in write speed isn't tested, because it reacts to lower
-// level CAN errors (CRC), which are hard to simulate, but it would be nice to add it
-
-TEST_F(TestMotorsController, TurnOnEStop)
-{
-  roboteqs_mock_->GetFrontDriver()->SetTurnOnEStop(65);
-  roboteqs_mock_->GetRearDriver()->SetTurnOnEStop(23);
-
-  ASSERT_NO_THROW(motors_controller_->TurnOnEStop());
-
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOnEStop(), 1);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOnEStop(), 1);
-}
-
-TEST_F(TestMotorsController, TurnOffEStop)
-{
-  roboteqs_mock_->GetFrontDriver()->SetTurnOffEStop(65);
-  roboteqs_mock_->GetRearDriver()->SetTurnOffEStop(23);
-
-  ASSERT_NO_THROW(motors_controller_->TurnOffEStop());
-
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOffEStop(), 1);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOffEStop(), 1);
-}
-
-TEST_F(TestMotorsController, TurnOnEStopTimeout)
-{
-  roboteqs_mock_->GetFrontDriver()->SetOnWriteWait<std::uint8_t>(0x200C, 0, 100000);
-  ASSERT_THROW(motors_controller_->TurnOnEStop(), std::runtime_error);
-}
-
-TEST_F(TestMotorsController, TurnOffEStopTimeout)
-{
-  roboteqs_mock_->GetFrontDriver()->SetOnWriteWait<std::uint8_t>(0x200D, 0, 100000);
-  ASSERT_THROW(motors_controller_->TurnOffEStop(), std::runtime_error);
-}
-
-TEST_F(TestMotorsController, SafetyStop)
-{
-  roboteqs_mock_->GetFrontDriver()->SetTurnOnSafetyStop(65);
-  roboteqs_mock_->GetRearDriver()->SetTurnOnSafetyStop(23);
-
-  bool front_driver_channel1_safety_stop = false;
-  bool rear_driver_channel1_safety_stop = false;
-
-  std::atomic_bool finish_test = false;
-
-  // Check if first channel was set in the meantime - not sure how robust this test will be - as
-  // safety stops for channel 1 and 2 are set just after one another, it is necessary to check value
-  // of the current channel set frequently (and performance can vary on different machines)
-  auto channel1_test_thread = std::thread([roboteqs_mock = roboteqs_mock_, &finish_test,
-                                           &front_driver_channel1_safety_stop,
-                                           &rear_driver_channel1_safety_stop]() {
-    while (true) {
-      if (
-        front_driver_channel1_safety_stop == false &&
-        roboteqs_mock->GetFrontDriver()->GetTurnOnSafetyStop() == 1) {
-        front_driver_channel1_safety_stop = true;
-      }
-
-      if (
-        rear_driver_channel1_safety_stop == false &&
-        roboteqs_mock->GetRearDriver()->GetTurnOnSafetyStop() == 1) {
-        rear_driver_channel1_safety_stop = true;
-      }
-
-      if (finish_test || (front_driver_channel1_safety_stop && rear_driver_channel1_safety_stop)) {
-        break;
-      }
-
-      std::this_thread::sleep_for(std::chrono::microseconds(10));
-    }
-  });
-
-  ASSERT_NO_THROW(motors_controller_->TurnOnSafetyStop());
-
-  finish_test = true;
-  channel1_test_thread.join();
-
-  ASSERT_TRUE(front_driver_channel1_safety_stop);
-  ASSERT_TRUE(rear_driver_channel1_safety_stop);
-
-  ASSERT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOnSafetyStop(), 2);
-  ASSERT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOnSafetyStop(), 2);
-}
-
-TEST_F(TestMotorsController, SafetyStopTimeout)
-{
-  roboteqs_mock_->GetFrontDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 100000);
-  ASSERT_THROW(motors_controller_->TurnOnSafetyStop(), std::runtime_error);
-}
-
-int main(int argc, char ** argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}
diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_canopen_manager.cpp
similarity index 96%
rename from panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp
rename to panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_canopen_manager.cpp
index 636956f3..cf846219 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_canopen_manager.cpp
@@ -19,7 +19,7 @@
 #include <gtest/gtest.h>
 #include <sys/socket.h>
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
 
 #include "utils/fake_can_socket.hpp"
 #include "utils/test_constants.hpp"
diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
similarity index 98%
rename from panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
rename to panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
index 022662bf..b95dafb3 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
@@ -20,8 +20,8 @@
 
 #include <gtest/gtest.h>
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp"
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
 
 #include "utils/fake_can_socket.hpp"
 #include "utils/roboteqs_mock.hpp"
diff --git a/panther_hardware_interfaces/test/utils/test_constants.hpp b/panther_hardware_interfaces/test/utils/test_constants.hpp
index ba1ff7ba..d7529b13 100644
--- a/panther_hardware_interfaces/test/utils/test_constants.hpp
+++ b/panther_hardware_interfaces/test/utils/test_constants.hpp
@@ -21,8 +21,8 @@
 #include <string>
 #include <vector>
 
-#include <panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp>
-#include <panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp>
+#include <panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp>
+#include <panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp>
 
 namespace panther_hardware_interfaces_test
 {

From 30eb97af4ad756a257679a1f2ae529143c3e69bd Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Mon, 26 Aug 2024 09:56:07 +0000
Subject: [PATCH 019/100] add panther robot driver tests

---
 .../test_panther_robot_driver.cpp             | 486 ++++++++++++++++++
 1 file changed, 486 insertions(+)
 create mode 100644 panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp

diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
new file mode 100644
index 00000000..81a61508
--- /dev/null
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
@@ -0,0 +1,486 @@
+// Copyright 2023 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 <chrono>
+#include <cstdint>
+#include <future>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <thread>
+
+#include <gmock/gmock.h>
+
+#include <panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp>
+#include <panther_hardware_interfaces/panther_system/robot_driver/driver.hpp>
+#include <panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp>
+
+#include "utils/fake_can_socket.hpp"
+#include "utils/test_constants.hpp"
+
+class MockDriver : public panther_hardware_interfaces::Driver
+{
+public:
+  MOCK_METHOD(std::future<void>, Boot, (), (override));
+  MOCK_METHOD(bool, IsCANError, (), (const, override));
+  MOCK_METHOD(bool, IsHeartbeatTimeout, (), (const, override));
+
+  MOCK_METHOD(panther_hardware_interfaces::DriverState, ReadDriverState, (), (override));
+  MOCK_METHOD(void, ResetScript, (), (override));
+  MOCK_METHOD(void, TurnOnEStop, (), (override));
+  MOCK_METHOD(void, TurnOffEStop, (), (override));
+
+  std::shared_ptr<panther_hardware_interfaces::MotorDriver> GetMotorDriver(
+    const std::string & name) override
+  {
+    return motor_drivers_.at(name);
+  }
+
+  void AddMotorDriver(
+    const std::string name,
+    std::shared_ptr<panther_hardware_interfaces::MotorDriver> motor_driver) override
+  {
+    motor_drivers_.emplace(name, motor_driver);
+  }
+
+private:
+  std::map<std::string, std::shared_ptr<panther_hardware_interfaces::MotorDriver>> motor_drivers_;
+};
+
+class MockMotorDriver : public panther_hardware_interfaces::MotorDriver
+{
+public:
+  MOCK_METHOD(panther_hardware_interfaces::MotorDriverState, ReadMotorState, (), (override));
+  MOCK_METHOD(void, SendCmd, (const std::int32_t cmd), (override));
+  MOCK_METHOD(void, TurnOnSafetyStop, (const std::uint8_t channel), (override));
+};
+
+class TestPantherRobotDriverInitialization : public ::testing::Test
+{
+public:
+  TestPantherRobotDriverInitialization()
+  {
+    can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
+      panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
+    can_socket_->Initialize();
+
+    front_driver_mock_ = std::make_shared<MockDriver>();
+    front_driver_mock_->AddMotorDriver(kLeftMotorDriverName, std::make_shared<MockMotorDriver>());
+    front_driver_mock_->AddMotorDriver(kRightMotorDriverName, std::make_shared<MockMotorDriver>());
+
+    rear_driver_mock_ = std::make_shared<MockDriver>();
+    rear_driver_mock_->AddMotorDriver(kLeftMotorDriverName, std::make_shared<MockMotorDriver>());
+    rear_driver_mock_->AddMotorDriver(kRightMotorDriverName, std::make_shared<MockMotorDriver>());
+
+    robot_driver_ = std::make_unique<panther_hardware_interfaces::PantherRobotDriver>(
+      front_driver_mock_, rear_driver_mock_, panther_hardware_interfaces_test::kCANopenSettings,
+      panther_hardware_interfaces_test::kDrivetrainSettings);
+  }
+
+  ~TestPantherRobotDriverInitialization() {}
+
+protected:
+  static constexpr char kFrontDriverName[] = "front";
+  static constexpr char kRearDriverName[] = "rear";
+  static constexpr char kLeftMotorDriverName[] = "left";
+  static constexpr char kRightMotorDriverName[] = "right";
+
+  std::shared_ptr<MockDriver> front_driver_mock_;
+  std::shared_ptr<MockDriver> rear_driver_mock_;
+  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
+  std::unique_ptr<panther_hardware_interfaces::PantherRobotDriver> robot_driver_;
+  std::shared_ptr<MockMotorDriver> fl_motor_driver_;
+  std::shared_ptr<MockMotorDriver> fr_motor_driver_;
+  std::shared_ptr<MockMotorDriver> rl_motor_driver_;
+  std::shared_ptr<MockMotorDriver> rr_motor_driver_;
+};
+
+TEST_F(TestPantherRobotDriverInitialization, Initialize)
+{
+  ASSERT_NO_THROW(robot_driver_->Initialize());
+  ASSERT_NO_THROW(robot_driver_->Deinitialize());
+
+  // Check if deinitialization worked correctly - initialize once again
+  ASSERT_NO_THROW(robot_driver_->Initialize());
+  ASSERT_NO_THROW(robot_driver_->Deinitialize());
+}
+
+TEST_F(TestPantherRobotDriverInitialization, Activate)
+{
+  EXPECT_CALL(*front_driver_mock_, Boot()).Times(1);
+  EXPECT_CALL(*rear_driver_mock_, Boot()).Times(1);
+  EXPECT_CALL(*front_driver_mock_, ResetScript()).Times(1);
+  EXPECT_CALL(*rear_driver_mock_, ResetScript()).Times(1);
+  EXPECT_CALL(*fl_motor_driver_, SendCmd(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*fr_motor_driver_, SendCmd(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*rl_motor_driver_, SendCmd(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*rr_motor_driver_, SendCmd(::testing::Eq(0))).Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->Initialize());
+  ASSERT_NO_THROW(robot_driver_->Activate());
+}
+
+class TestPantherRobotDriver : public TestPantherRobotDriverInitialization
+{
+public:
+  TestPantherRobotDriver()
+  {
+    robot_driver_->Initialize();
+    robot_driver_->Activate();
+  }
+
+  ~TestPantherRobotDriver() { robot_driver_->Deinitialize(); }
+};
+
+TEST_F(TestPantherRobotDriver, UpdateCommunicationState)
+{
+  EXPECT_CALL(*front_driver_mock_, IsCANError()).Times(1);
+  EXPECT_CALL(*front_driver_mock_, IsHeartbeatTimeout()).Times(1);
+  EXPECT_CALL(*rear_driver_mock_, IsCANError()).Times(1);
+  EXPECT_CALL(*rear_driver_mock_, IsHeartbeatTimeout()).Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
+}
+
+TEST_F(TestPantherRobotDriver, UpdateCommunicationStateCANErorr)
+{
+  EXPECT_CALL(*front_driver_mock_, IsCANError()).WillOnce(::testing::Return(true));
+  EXPECT_CALL(*rear_driver_mock_, IsCANError()).WillOnce(::testing::Return(true));
+
+  ASSERT_THROW(robot_driver_->UpdateCommunicationState(), std::runtime_error);
+
+  EXPECT_TRUE(robot_driver_->GetData(kFrontDriverName).IsCANError());
+  EXPECT_TRUE(robot_driver_->GetData(kRearDriverName).IsCANError());
+}
+
+TEST_F(TestPantherRobotDriver, UpdateCommunicationStateHeartbeatTimeout)
+{
+  EXPECT_CALL(*front_driver_mock_, IsHeartbeatTimeout()).WillOnce(::testing::Return(true));
+  EXPECT_CALL(*rear_driver_mock_, IsHeartbeatTimeout()).WillOnce(::testing::Return(true));
+
+  ASSERT_THROW(robot_driver_->UpdateCommunicationState(), std::runtime_error);
+
+  EXPECT_TRUE(robot_driver_->GetData(kFrontDriverName).IsHeartbeatTimeout());
+  EXPECT_TRUE(robot_driver_->GetData(kRearDriverName).IsHeartbeatTimeout());
+}
+
+TEST_F(TestPantherRobotDriver, UpdateMotorsState)
+{
+  using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  using panther_hardware_interfaces_test::kRbtqPosFbToRad;
+  using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+
+  const std::int32_t fl_pos = 101;
+  const std::int32_t fl_vel = 102;
+  const std::int32_t fl_current = 103;
+  const std::int32_t fr_pos = 201;
+  const std::int32_t fr_vel = 202;
+  const std::int32_t fr_current = 203;
+  const std::int32_t rl_pos = 301;
+  const std::int32_t rl_vel = 302;
+  const std::int32_t rl_current = 303;
+  const std::int32_t rr_pos = 401;
+  const std::int32_t rr_vel = 402;
+  const std::int32_t rr_current = 403;
+
+  ON_CALL(*fl_motor_driver_, ReadMotorState())
+    .WillByDefault(::testing::Return(
+      panther_hardware_interfaces::MotorDriverState({fl_pos, fl_vel, fl_current, {0, 0}, {0, 0}})));
+  ON_CALL(*fr_motor_driver_, ReadMotorState())
+    .WillByDefault(::testing::Return(
+      panther_hardware_interfaces::MotorDriverState({fr_pos, fr_vel, fr_current, {0, 0}, {0, 0}})));
+  ON_CALL(*rl_motor_driver_, ReadMotorState())
+    .WillByDefault(::testing::Return(
+      panther_hardware_interfaces::MotorDriverState({rl_pos, rl_vel, rl_current, {0, 0}, {0, 0}})));
+  ON_CALL(*rr_motor_driver_, ReadMotorState())
+    .WillByDefault(::testing::Return(
+      panther_hardware_interfaces::MotorDriverState({rr_pos, rr_vel, rr_current, {0, 0}, {0, 0}})));
+
+  robot_driver_->UpdateMotorsState();
+
+  const auto & fl = robot_driver_->GetData(kFrontDriverName).GetMotorState(kLeftMotorDriverName);
+  const auto & fr = robot_driver_->GetData(kFrontDriverName).GetMotorState(kRightMotorDriverName);
+  const auto & rl = robot_driver_->GetData(kRearDriverName).GetMotorState(kLeftMotorDriverName);
+  const auto & rr = robot_driver_->GetData(kRearDriverName).GetMotorState(kRightMotorDriverName);
+
+  EXPECT_FLOAT_EQ(fl.GetPosition(), fl_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(fl.GetVelocity(), fl_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(fl.GetTorque(), fl_current * kRbtqCurrentFbToNewtonMeters);
+
+  EXPECT_FLOAT_EQ(fr.GetPosition(), fr_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(fr.GetVelocity(), fr_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(fr.GetTorque(), fr_current * kRbtqCurrentFbToNewtonMeters);
+
+  EXPECT_FLOAT_EQ(rl.GetPosition(), rl_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(rl.GetVelocity(), rl_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(rl.GetTorque(), rl_current * kRbtqCurrentFbToNewtonMeters);
+
+  EXPECT_FLOAT_EQ(rr.GetPosition(), rr_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(rr.GetVelocity(), rr_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(rr.GetTorque(), rr_current * kRbtqCurrentFbToNewtonMeters);
+}
+
+TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimestamps)
+{
+  robot_driver_->UpdateMotorsState();
+
+  std::this_thread::sleep_for(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
+    std::chrono::milliseconds(10));
+
+  robot_driver_->UpdateMotorsState();
+
+  EXPECT_FALSE(robot_driver_->GetData(kFrontDriverName).IsMotorStatesDataTimedOut());
+  EXPECT_FALSE(robot_driver_->GetData(kRearDriverName).IsMotorStatesDataTimedOut());
+}
+
+TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimeout)
+{
+  timespec current_time;
+  clock_gettime(CLOCK_MONOTONIC, &current_time);
+
+  panther_hardware_interfaces::MotorDriverState state;
+  state.pos_timestamp = current_time;
+
+  EXPECT_CALL(*fl_motor_driver_, ReadMotorState()).WillOnce(::testing::Return(state));
+  EXPECT_CALL(*rl_motor_driver_, ReadMotorState()).WillOnce(::testing::Return(state));
+
+  // sleep for pdo_motor_states_timeout_ms + 10ms
+  std::this_thread::sleep_for(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
+    std::chrono::milliseconds(10));
+
+  robot_driver_->UpdateMotorsState();
+
+  EXPECT_TRUE(robot_driver_->GetData(kFrontDriverName).IsMotorStatesDataTimedOut());
+  EXPECT_TRUE(robot_driver_->GetData(kRearDriverName).IsMotorStatesDataTimedOut());
+  EXPECT_TRUE(robot_driver_->GetData(kFrontDriverName).IsError());
+  EXPECT_TRUE(robot_driver_->GetData(kRearDriverName).IsError());
+}
+
+TEST_F(TestPantherRobotDriver, UpdateDriverState)
+{
+  using panther_hardware_interfaces_test::DriverFaultFlags;
+  using panther_hardware_interfaces_test::DriverRuntimeErrors;
+  using panther_hardware_interfaces_test::DriverScriptFlags;
+
+  const std::int16_t f_temp = 30;
+  const std::int16_t r_temp = 32;
+  const std::int16_t f_heatsink_temp = 31;
+  const std::int16_t r_heatsink_temp = 33;
+  const std::uint16_t f_volt = 400;
+  const std::uint16_t r_volt = 430;
+  const std::int16_t f_battery_current_1 = 10;
+  const std::int16_t r_battery_current_1 = 30;
+  const std::int16_t f_battery_current_2 = 30;
+  const std::int16_t r_battery_current_2 = 40;
+
+  const std::uint8_t fault_flag_overheat = 0x01;
+  const std::uint8_t fault_flag_overvoltage = 0x02;
+  const std::uint8_t script_flag_amp_limiter = 0x01;
+  const std::uint8_t script_flag_encoder_disconnected = 0x02;
+  const std::uint8_t runtime_error_loop_error = 0x01;
+  const std::uint8_t runtime_error_safety_stop_active = 0x02;
+  const std::uint8_t runtime_error_forward_limit_triggered = 0x04;
+  const std::uint8_t runtime_error_reverse_limit_triggered = 0x08;
+
+  ON_CALL(*front_driver_mock_, ReadDriverState())
+    .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
+      {fault_flag_overheat,
+       script_flag_amp_limiter,
+       runtime_error_loop_error,
+       runtime_error_safety_stop_active,
+       f_battery_current_1,
+       f_battery_current_2,
+       f_volt,
+       f_temp,
+       f_heatsink_temp,
+       {0, 0},
+       {0, 0}})));
+  ON_CALL(*rear_driver_mock_, ReadDriverState())
+    .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
+      {fault_flag_overvoltage,
+       script_flag_encoder_disconnected,
+       runtime_error_forward_limit_triggered,
+       runtime_error_reverse_limit_triggered,
+       r_battery_current_1,
+       r_battery_current_2,
+       r_volt,
+       r_temp,
+       r_heatsink_temp,
+       {0, 0},
+       {0, 0}})));
+
+  robot_driver_->UpdateDriversState();
+
+  const auto & front = robot_driver_->GetData(kFrontDriverName);
+  const auto & rear = robot_driver_->GetData(kRearDriverName);
+  const auto & front_driver_state = robot_driver_->GetData(kFrontDriverName).GetDriverState();
+  const auto & rear_driver_state = robot_driver_->GetData(kRearDriverName).GetDriverState();
+
+  EXPECT_EQ(static_cast<std::int16_t>(front_driver_state.GetTemperature()), f_temp);
+  EXPECT_EQ(
+    static_cast<std::int16_t>(front_driver_state.GetHeatsinkTemperature()), f_heatsink_temp);
+  EXPECT_EQ(static_cast<std::uint16_t>(front_driver_state.GetVoltage() * 10.0), f_volt);
+  EXPECT_EQ(
+    static_cast<std::int16_t>(front_driver_state.GetCurrent() * 10.0),
+    f_battery_current_1 + f_battery_current_2);
+
+  EXPECT_EQ(static_cast<std::int16_t>(rear_driver_state.GetTemperature()), r_temp);
+  EXPECT_EQ(static_cast<std::int16_t>(rear_driver_state.GetHeatsinkTemperature()), r_heatsink_temp);
+  EXPECT_EQ(static_cast<std::uint16_t>(rear_driver_state.GetVoltage() * 10.0), r_volt);
+  EXPECT_EQ(
+    static_cast<std::int16_t>(rear_driver_state.GetCurrent() * 10.0),
+    r_battery_current_1 + r_battery_current_2);
+
+  EXPECT_TRUE(front.GetFaultFlag().GetMessage().overheat);
+  EXPECT_TRUE(front.GetScriptFlag().GetMessage().encoder_disconnected);
+  EXPECT_TRUE(front.GetRightRuntimeError().GetMessage().loop_error);
+  EXPECT_TRUE(front.GetLeftRuntimeError().GetMessage().safety_stop_active);
+
+  EXPECT_TRUE(rear.GetFaultFlag().GetMessage().overvoltage);
+  EXPECT_TRUE(rear.GetScriptFlag().GetMessage().amp_limiter);
+  EXPECT_TRUE(rear.GetRightRuntimeError().GetMessage().forward_limit_triggered);
+  EXPECT_TRUE(rear.GetLeftRuntimeError().GetMessage().reverse_limit_triggered);
+}
+
+TEST_F(TestPantherRobotDriver, UpdateDriverStateTimestamps)
+{
+  robot_driver_->UpdateDriversState();
+
+  std::this_thread::sleep_for(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
+    std::chrono::milliseconds(10));
+
+  robot_driver_->UpdateDriversState();
+
+  EXPECT_FALSE(robot_driver_->GetData(kFrontDriverName).IsDriverStateDataTimedOut());
+  EXPECT_FALSE(robot_driver_->GetData(kRearDriverName).IsDriverStateDataTimedOut());
+}
+
+TEST_F(TestPantherRobotDriver, UpdateDriverStateTimeout)
+{
+  timespec current_time;
+  clock_gettime(CLOCK_MONOTONIC, &current_time);
+
+  panther_hardware_interfaces::DriverState state;
+  state.flags_current_timestamp = current_time;
+
+  EXPECT_CALL(*front_driver_mock_, ReadDriverState()).WillOnce(::testing::Return(state));
+  EXPECT_CALL(*rear_driver_mock_, ReadDriverState()).WillOnce(::testing::Return(state));
+
+  std::this_thread::sleep_for(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
+    std::chrono::milliseconds(10));
+
+  robot_driver_->UpdateDriversState();
+
+  EXPECT_TRUE(robot_driver_->GetData(kFrontDriverName).IsDriverStateDataTimedOut());
+  EXPECT_TRUE(robot_driver_->GetData(kRearDriverName).IsDriverStateDataTimedOut());
+  EXPECT_TRUE(robot_driver_->GetData(kFrontDriverName).IsError());
+  EXPECT_TRUE(robot_driver_->GetData(kRearDriverName).IsError());
+}
+
+TEST_F(TestPantherRobotDriver, SendSpeedCommands)
+{
+  using panther_hardware_interfaces_test::kRadPerSecToRbtqCmd;
+
+  const float fl_v = 0.1;
+  const float fr_v = 0.2;
+  const float rl_v = 0.3;
+  const float rr_v = 0.4;
+
+  EXPECT_CALL(
+    *fl_motor_driver_,
+    SendCmd(::testing::Eq(static_cast<std::int32_t>(fl_v * kRadPerSecToRbtqCmd))))
+    .Times(1);
+  EXPECT_CALL(
+    *fr_motor_driver_,
+    SendCmd(::testing::Eq(static_cast<std::int32_t>(fr_v * kRadPerSecToRbtqCmd))))
+    .Times(1);
+  EXPECT_CALL(
+    *rl_motor_driver_,
+    SendCmd(::testing::Eq(static_cast<std::int32_t>(rl_v * kRadPerSecToRbtqCmd))))
+    .Times(1);
+  EXPECT_CALL(
+    *rr_motor_driver_,
+    SendCmd(::testing::Eq(static_cast<std::int32_t>(rr_v * kRadPerSecToRbtqCmd))))
+    .Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->SendSpeedCommands(fl_v, fr_v, rl_v, rr_v));
+}
+
+TEST_F(TestPantherRobotDriver, TurnOnEStop)
+{
+  EXPECT_CALL(*front_driver_mock_, TurnOnEStop()).Times(1);
+  EXPECT_CALL(*rear_driver_mock_, TurnOnEStop()).Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->TurnOnEStop());
+}
+
+TEST_F(TestPantherRobotDriver, TurnOffEStop)
+{
+  EXPECT_CALL(*front_driver_mock_, TurnOffEStop()).Times(1);
+  EXPECT_CALL(*rear_driver_mock_, TurnOffEStop()).Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->TurnOffEStop());
+}
+
+TEST_F(TestPantherRobotDriver, TurnOnEStopError)
+{
+  EXPECT_CALL(*front_driver_mock_, TurnOnEStop())
+    .WillOnce(::testing::Throw(std::runtime_error("")));
+  EXPECT_CALL(*rear_driver_mock_, TurnOnEStop()).Times(0);
+
+  ASSERT_THROW(robot_driver_->TurnOnEStop(), std::runtime_error);
+}
+
+TEST_F(TestPantherRobotDriver, TurnOffEStopError)
+{
+  EXPECT_CALL(*front_driver_mock_, TurnOffEStop())
+    .WillOnce(::testing::Throw(std::runtime_error("")));
+  EXPECT_CALL(*rear_driver_mock_, TurnOffEStop()).Times(0);
+
+  ASSERT_THROW(robot_driver_->TurnOffEStop(), std::runtime_error);
+}
+
+TEST_F(TestPantherRobotDriver, SafetyStop)
+{
+  using panther_hardware_interfaces::PantherRobotDriver::kLeftChannel;
+  using panther_hardware_interfaces::PantherRobotDriver::kRightChannel;
+
+  EXPECT_CALL(*fl_motor_driver_, TurnOnSafetyStop(::testing::Eq(kLeftChannel))).Times(1);
+  EXPECT_CALL(*fr_motor_driver_, TurnOnSafetyStop(::testing::Eq(kRightChannel))).Times(1);
+  EXPECT_CALL(*rl_motor_driver_, TurnOnSafetyStop(::testing::Eq(kLeftChannel))).Times(1);
+  EXPECT_CALL(*rr_motor_driver_, TurnOnSafetyStop(::testing::Eq(kRightChannel))).Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->TurnOnSafetyStop());
+}
+
+TEST_F(TestPantherRobotDriver, SafetyStopError)
+{
+  EXPECT_CALL(*fl_motor_driver_, TurnOnSafetyStop(::testing::Eq(kLeftChannel)))
+    .WillOnce(::testing::Throw(std::runtime_error("")));
+  EXPECT_CALL(*fr_motor_driver_, TurnOnSafetyStop(::testing::Eq(kRightChannel))).Times(0);
+  EXPECT_CALL(*rl_motor_driver_, TurnOnSafetyStop(::testing::Eq(kLeftChannel))).Times(0);
+  EXPECT_CALL(*rr_motor_driver_, TurnOnSafetyStop(::testing::Eq(kRightChannel))).Times(0);
+
+  ASSERT_THROW(robot_driver_->TurnOnSafetyStop(), std::runtime_error);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

From 0d52315454c8fa2763b3f901b2fe4d5d55e47fe7 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Mon, 26 Aug 2024 13:18:39 +0000
Subject: [PATCH 020/100] robot driver abstraction and panther implementation

---
 .../robot_driver/robot_driver.hpp             |  51 ++++-
 ...motors_controller.cpp => robot_driver.cpp} | 146 +++++++++------
 .../test_panther_robot_driver.cpp             | 174 +++++++++++-------
 .../test/utils/test_constants.hpp             |   2 +-
 4 files changed, 238 insertions(+), 135 deletions(-)
 rename panther_hardware_interfaces/src/panther_system/robot_driver/{motors_controller.cpp => robot_driver.cpp} (55%)

diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
index f8378155..aeb65c2e 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
@@ -24,16 +24,30 @@
 namespace panther_hardware_interfaces
 {
 
+struct PantherMotorNames
+{
+  static constexpr char LEFT[] = "left";
+  static constexpr char RIGHT[] = "right";
+};
+
+struct PantherDriverNames
+{
+  static constexpr char FRONT[] = "front";
+  static constexpr char REAR[] = "rear";
+};
+
+struct PantherMotorChannels
+{
+  static constexpr std::uint8_t LEFT = 2;
+  static constexpr std::uint8_t RIGHT = 1;
+};
+
 /**
  * @brief Abstract class for managing robot drivers.
  */
 class RobotDriver
 {
 public:
-  RobotDriver() = default;
-
-  ~RobotDriver() { Deinitialize(); }
-
   /**
    * @brief Initialize robot driver
    *
@@ -137,9 +151,16 @@ class PantherRobotDriver : public RobotDriver
 {
 public:
   PantherRobotDriver(
-    const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings);
+    const std::shared_ptr<Driver> front_driver, const std::shared_ptr<Driver> rear_driver,
+    const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings,
+    const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000));
 
-  ~PantherRobotDriver() = default;
+  ~PantherRobotDriver()
+  {
+    front_driver_.reset();
+    rear_driver_.reset();
+    Deinitialize();
+  };
 
   /**
    * @brief Starts CAN communication and waits for boot to finish
@@ -182,6 +203,14 @@ class PantherRobotDriver : public RobotDriver
    */
   void UpdateDriversState() override;
 
+  /**
+   * @brief Get data feedback from the driver
+   *
+   * @param name name of the data to get
+   *
+   * @return data feedback
+   * @exception std::runtime_error if data with the given name does not exist
+   */
   const RoboteqData & GetData(const std::string & name) override;
 
   /**
@@ -227,14 +256,17 @@ class PantherRobotDriver : public RobotDriver
 
 private:
   void SetMotorsStates(
-    RoboteqData & data, const RoboteqMotorsStates & states, const timespec & current_time);
-  void SetDriverState(
-    RoboteqData & data, const RoboteqDriverState & state, const timespec & current_time);
+    RoboteqData & data, const MotorDriverState & front_state, const MotorDriverState & rear_state,
+    const timespec & current_time);
+  void SetDriverState(RoboteqData & data, const DriverState & state, const timespec & current_time);
 
   bool initialized_ = false;
 
   CANopenManager canopen_manager_;
 
+  std::shared_ptr<Driver> front_driver_;
+  std::shared_ptr<Driver> rear_driver_;
+
   RoboteqData front_data_;
   RoboteqData rear_data_;
 
@@ -242,6 +274,7 @@ class PantherRobotDriver : public RobotDriver
 
   const std::chrono::milliseconds pdo_motor_states_timeout_ms_;
   const std::chrono::milliseconds pdo_driver_state_timeout_ms_;
+  const std::chrono::milliseconds activate_wait_time_;
 };
 
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/motors_controller.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/robot_driver.cpp
similarity index 55%
rename from panther_hardware_interfaces/src/panther_system/robot_driver/motors_controller.cpp
rename to panther_hardware_interfaces/src/panther_system/robot_driver/robot_driver.cpp
index 4c366606..1fb31adc 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/motors_controller.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/robot_driver.cpp
@@ -16,8 +16,11 @@
 
 #include <chrono>
 #include <ctime>
+#include <memory>
 #include <stdexcept>
+#include <string>
 #include <thread>
+#include <utility>
 
 #include "lely/util/chrono.hpp"
 
@@ -28,18 +31,23 @@
 namespace panther_hardware_interfaces
 {
 
-MotorsController::MotorsController(
-  const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings)
+PantherRobotDriver::PantherRobotDriver(
+  const std::shared_ptr<Driver> front_driver, const std::shared_ptr<Driver> rear_driver,
+  const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings,
+  const std::chrono::milliseconds activate_wait_time)
 : canopen_manager_(canopen_settings),
+  front_driver_(std::move(front_driver)),
+  rear_driver_(std::move(rear_driver)),
   front_data_(drivetrain_settings),
   rear_data_(drivetrain_settings),
   roboteq_vel_cmd_converter_(drivetrain_settings),
   pdo_motor_states_timeout_ms_(canopen_settings.pdo_motor_states_timeout_ms),
-  pdo_driver_state_timeout_ms_(canopen_settings.pdo_driver_state_timeout_ms)
+  pdo_driver_state_timeout_ms_(canopen_settings.pdo_driver_state_timeout_ms),
+  activate_wait_time_(activate_wait_time)
 {
 }
 
-void MotorsController::Initialize()
+void PantherRobotDriver::Initialize()
 {
   if (initialized_) {
     return;
@@ -47,6 +55,8 @@ void MotorsController::Initialize()
 
   try {
     canopen_manager_.Initialize();
+    front_driver_->Boot();
+    rear_driver_->Boot();
   } catch (const std::runtime_error & e) {
     throw e;
   }
@@ -54,66 +64,75 @@ void MotorsController::Initialize()
   initialized_ = true;
 }
 
-void MotorsController::Deinitialize()
+void PantherRobotDriver::Deinitialize()
 {
   canopen_manager_.Deinitialize();
   initialized_ = false;
 }
 
-void MotorsController::Activate()
+void PantherRobotDriver::Activate()
 {
   // Activation procedure - it is necessary to first reset scripts, wait for a bit (1 second)
   // and then send 0 commands for some time (also 1 second)
 
   try {
-    canopen_manager_.GetFrontDriver()->ResetRoboteqScript();
+    front_driver_->ResetScript();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Front driver reset Roboteq script exception: " + std::string(e.what()));
   }
 
   try {
-    canopen_manager_.GetRearDriver()->ResetRoboteqScript();
+    rear_driver_->ResetScript();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Rear driver reset Roboteq script exception: " + std::string(e.what()));
   }
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  std::this_thread::sleep_for(activate_wait_time_);
 
   try {
-    canopen_manager_.GetFrontDriver()->SendRoboteqCmd(0, 0);
+    front_driver_->GetMotorDriver(PantherMotorNames::LEFT)->SendCmdVel(0);
+    front_driver_->GetMotorDriver(PantherMotorNames::RIGHT)->SendCmdVel(0);
   } catch (const std::runtime_error & e) {
     throw std::runtime_error("Front driver send 0 command exception: " + std::string(e.what()));
   }
 
   try {
-    canopen_manager_.GetRearDriver()->SendRoboteqCmd(0, 0);
+    rear_driver_->GetMotorDriver(PantherMotorNames::LEFT)->SendCmdVel(0);
+    rear_driver_->GetMotorDriver(PantherMotorNames::RIGHT)->SendCmdVel(0);
   } catch (const std::runtime_error & e) {
     throw std::runtime_error("Rear driver send 0 command exception: " + std::string(e.what()));
   }
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  std::this_thread::sleep_for(activate_wait_time_);
 }
 
-void MotorsController::UpdateCommunicationState()
+void PantherRobotDriver::UpdateCommunicationState()
 {
-  front_data_.SetCANError(canopen_manager_.GetFrontDriver()->IsCANError());
-  rear_data_.SetCANError(canopen_manager_.GetRearDriver()->IsCANError());
+  front_data_.SetCANError(front_driver_->IsCANError());
+  rear_data_.SetCANError(rear_driver_->IsCANError());
 
-  front_data_.SetHeartbeatTimeout(canopen_manager_.GetFrontDriver()->IsHeartbeatTimeout());
-  rear_data_.SetHeartbeatTimeout(canopen_manager_.GetRearDriver()->IsHeartbeatTimeout());
+  front_data_.SetHeartbeatTimeout(front_driver_->IsHeartbeatTimeout());
+  rear_data_.SetHeartbeatTimeout(rear_driver_->IsHeartbeatTimeout());
 }
 
-void MotorsController::UpdateMotorsState()
+void PantherRobotDriver::UpdateMotorsState()
 {
   timespec current_time;
   clock_gettime(CLOCK_MONOTONIC, &current_time);
 
-  SetMotorsStates(
-    front_data_, canopen_manager_.GetFrontDriver()->ReadRoboteqMotorsStates(), current_time);
-  SetMotorsStates(
-    rear_data_, canopen_manager_.GetRearDriver()->ReadRoboteqMotorsStates(), current_time);
+  const auto fl_state =
+    front_driver_->GetMotorDriver(PantherMotorNames::LEFT)->ReadMotorDriverState();
+  const auto fr_state =
+    front_driver_->GetMotorDriver(PantherMotorNames::RIGHT)->ReadMotorDriverState();
+  const auto rl_state =
+    rear_driver_->GetMotorDriver(PantherMotorNames::LEFT)->ReadMotorDriverState();
+  const auto rr_state =
+    rear_driver_->GetMotorDriver(PantherMotorNames::RIGHT)->ReadMotorDriverState();
+
+  SetMotorsStates(front_data_, fl_state, fr_state, current_time);
+  SetMotorsStates(rear_data_, rl_state, rr_state, current_time);
 
   UpdateCommunicationState();
 
@@ -126,15 +145,13 @@ void MotorsController::UpdateMotorsState()
   }
 }
 
-void MotorsController::UpdateDriversState()
+void PantherRobotDriver::UpdateDriversState()
 {
   timespec current_time;
   clock_gettime(CLOCK_MONOTONIC, &current_time);
 
-  SetDriverState(
-    front_data_, canopen_manager_.GetFrontDriver()->ReadRoboteqDriverState(), current_time);
-  SetDriverState(
-    rear_data_, canopen_manager_.GetRearDriver()->ReadRoboteqDriverState(), current_time);
+  SetDriverState(front_data_, front_driver_->ReadDriverState(), current_time);
+  SetDriverState(rear_data_, rear_driver_->ReadDriverState(), current_time);
 
   UpdateCommunicationState();
 
@@ -147,99 +164,116 @@ void MotorsController::UpdateDriversState()
   }
 }
 
-void MotorsController::SendSpeedCommands(
+const RoboteqData & PantherRobotDriver::GetData(const std::string & name)
+{
+  if (name == PantherDriverNames::FRONT) {
+    return front_data_;
+  } else if (name == PantherDriverNames::REAR) {
+    return rear_data_;
+  } else {
+    throw std::runtime_error("Data with name '" + name + "' does not exist.");
+  }
+}
+
+void PantherRobotDriver::SendSpeedCommands(
   const float speed_fl, const float speed_fr, const float speed_rl, const float speed_rr)
 {
   // Channel 1 - right motor, Channel 2 - left motor
   try {
-    canopen_manager_.GetFrontDriver()->SendRoboteqCmd(
-      roboteq_vel_cmd_converter_.Convert(speed_fr), roboteq_vel_cmd_converter_.Convert(speed_fl));
+    front_driver_->GetMotorDriver(PantherMotorNames::LEFT)
+      ->SendCmdVel(roboteq_vel_cmd_converter_.Convert(speed_fl));
+    front_driver_->GetMotorDriver(PantherMotorNames::RIGHT)
+      ->SendCmdVel(roboteq_vel_cmd_converter_.Convert(speed_fr));
   } catch (const std::runtime_error & e) {
     throw std::runtime_error("Front driver send Roboteq cmd failed: " + std::string(e.what()));
   }
   try {
-    canopen_manager_.GetRearDriver()->SendRoboteqCmd(
-      roboteq_vel_cmd_converter_.Convert(speed_rr), roboteq_vel_cmd_converter_.Convert(speed_rl));
+    rear_driver_->GetMotorDriver(PantherMotorNames::LEFT)
+      ->SendCmdVel(roboteq_vel_cmd_converter_.Convert(speed_rl));
+    rear_driver_->GetMotorDriver(PantherMotorNames::RIGHT)
+      ->SendCmdVel(roboteq_vel_cmd_converter_.Convert(speed_rr));
   } catch (const std::runtime_error & e) {
     throw std::runtime_error("Rear driver send Roboteq cmd failed: " + std::string(e.what()));
   }
 
-  if (canopen_manager_.GetFrontDriver()->IsCANError()) {
+  if (front_driver_->IsCANError()) {
     throw std::runtime_error(
       "CAN error detected on the front driver when trying to write speed commands.");
   }
-  if (canopen_manager_.GetRearDriver()->IsCANError()) {
+  if (rear_driver_->IsCANError()) {
     throw std::runtime_error(
       "CAN error detected on the rear driver when trying to write speed commands.");
   }
 }
 
-void MotorsController::TurnOnEStop()
+void PantherRobotDriver::TurnOnEStop()
 {
   try {
-    canopen_manager_.GetFrontDriver()->TurnOnEStop();
+    front_driver_->TurnOnEStop();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Failed to turn on E-stop on the front driver: " + std::string(e.what()));
   }
   try {
-    canopen_manager_.GetRearDriver()->TurnOnEStop();
+    rear_driver_->TurnOnEStop();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Failed to turn on E-stop on the rear driver: " + std::string(e.what()));
   }
 }
 
-void MotorsController::TurnOffEStop()
+void PantherRobotDriver::TurnOffEStop()
 {
   try {
-    canopen_manager_.GetFrontDriver()->TurnOffEStop();
+    front_driver_->TurnOffEStop();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Failed to turn off E-stop on the front driver: " + std::string(e.what()));
   }
   try {
-    canopen_manager_.GetRearDriver()->TurnOffEStop();
+    rear_driver_->TurnOffEStop();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Failed to turn off E-stop on the rear driver: " + std::string(e.what()));
   }
 }
 
-void MotorsController::TurnOnSafetyStop()
+void PantherRobotDriver::TurnOnSafetyStop()
 {
   try {
-    canopen_manager_.GetFrontDriver()->TurnOnSafetyStopChannel1();
-    canopen_manager_.GetFrontDriver()->TurnOnSafetyStopChannel2();
+    front_driver_->GetMotorDriver(PantherMotorNames::LEFT)->TurnOnSafetyStop();
+    front_driver_->GetMotorDriver(PantherMotorNames::RIGHT)->TurnOnSafetyStop();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Failed to turn on safety stop on the front driver: " + std::string(e.what()));
   }
   try {
-    canopen_manager_.GetRearDriver()->TurnOnSafetyStopChannel1();
-    canopen_manager_.GetRearDriver()->TurnOnSafetyStopChannel2();
+    rear_driver_->GetMotorDriver(PantherMotorNames::LEFT)->TurnOnSafetyStop();
+    rear_driver_->GetMotorDriver(PantherMotorNames::RIGHT)->TurnOnSafetyStop();
   } catch (const std::runtime_error & e) {
     throw std::runtime_error(
       "Failed to turn on safety stop on the rear driver: " + std::string(e.what()));
   }
 }
 
-void MotorsController::SetMotorsStates(
-  RoboteqData & data, const RoboteqMotorsStates & states, const timespec & current_time)
+void PantherRobotDriver::SetMotorsStates(
+  RoboteqData & data, const MotorDriverState & front_state, const MotorDriverState & rear_state,
+  const timespec & current_time)
 {
-  bool data_timed_out =
-    (lely::util::from_timespec(current_time) - lely::util::from_timespec(states.pos_timestamp) >
-     pdo_motor_states_timeout_ms_) ||
-    (lely::util::from_timespec(current_time) -
-       lely::util::from_timespec(states.vel_current_timestamp) >
-     pdo_motor_states_timeout_ms_);
+  // TODO figure out both motors timestamps
+  bool data_timed_out = (lely::util::from_timespec(current_time) -
+                           lely::util::from_timespec(front_state.pos_timestamp) >
+                         pdo_motor_states_timeout_ms_) ||
+                        (lely::util::from_timespec(current_time) -
+                           lely::util::from_timespec(front_state.vel_current_timestamp) >
+                         pdo_motor_states_timeout_ms_);
 
   // Channel 1 - right, Channel 2 - left
-  data.SetMotorsStates(states.motor_2, states.motor_1, data_timed_out);
+  data.SetMotorsStates(front_state, rear_state, data_timed_out);
 }
 
-void MotorsController::SetDriverState(
-  RoboteqData & data, const RoboteqDriverState & state, const timespec & current_time)
+void PantherRobotDriver::SetDriverState(
+  RoboteqData & data, const DriverState & state, const timespec & current_time)
 {
   bool data_timed_out = (lely::util::from_timespec(current_time) -
                            lely::util::from_timespec(state.flags_current_timestamp) >
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
index 81a61508..7a795ff7 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
@@ -61,9 +61,9 @@ class MockDriver : public panther_hardware_interfaces::Driver
 class MockMotorDriver : public panther_hardware_interfaces::MotorDriver
 {
 public:
-  MOCK_METHOD(panther_hardware_interfaces::MotorDriverState, ReadMotorState, (), (override));
-  MOCK_METHOD(void, SendCmd, (const std::int32_t cmd), (override));
-  MOCK_METHOD(void, TurnOnSafetyStop, (const std::uint8_t channel), (override));
+  MOCK_METHOD(panther_hardware_interfaces::MotorDriverState, ReadMotorDriverState, (), (override));
+  MOCK_METHOD(void, SendCmdVel, (const std::int32_t cmd), (override));
+  MOCK_METHOD(void, TurnOnSafetyStop, (), (override));
 };
 
 class TestPantherRobotDriverInitialization : public ::testing::Test
@@ -75,17 +75,22 @@ class TestPantherRobotDriverInitialization : public ::testing::Test
       panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
     can_socket_->Initialize();
 
-    front_driver_mock_ = std::make_shared<MockDriver>();
-    front_driver_mock_->AddMotorDriver(kLeftMotorDriverName, std::make_shared<MockMotorDriver>());
-    front_driver_mock_->AddMotorDriver(kRightMotorDriverName, std::make_shared<MockMotorDriver>());
+    fl_motor_driver_ = std::make_shared<::testing::NiceMock<MockMotorDriver>>();
+    fr_motor_driver_ = std::make_shared<::testing::NiceMock<MockMotorDriver>>();
+    rl_motor_driver_ = std::make_shared<::testing::NiceMock<MockMotorDriver>>();
+    rr_motor_driver_ = std::make_shared<::testing::NiceMock<MockMotorDriver>>();
 
-    rear_driver_mock_ = std::make_shared<MockDriver>();
-    rear_driver_mock_->AddMotorDriver(kLeftMotorDriverName, std::make_shared<MockMotorDriver>());
-    rear_driver_mock_->AddMotorDriver(kRightMotorDriverName, std::make_shared<MockMotorDriver>());
+    front_driver_mock_ = std::make_shared<::testing::NiceMock<MockDriver>>();
+    front_driver_mock_->AddMotorDriver(kLeftMotorDriverName, fl_motor_driver_);
+    front_driver_mock_->AddMotorDriver(kRightMotorDriverName, fr_motor_driver_);
+
+    rear_driver_mock_ = std::make_shared<::testing::NiceMock<MockDriver>>();
+    rear_driver_mock_->AddMotorDriver(kLeftMotorDriverName, rl_motor_driver_);
+    rear_driver_mock_->AddMotorDriver(kRightMotorDriverName, rr_motor_driver_);
 
     robot_driver_ = std::make_unique<panther_hardware_interfaces::PantherRobotDriver>(
       front_driver_mock_, rear_driver_mock_, panther_hardware_interfaces_test::kCANopenSettings,
-      panther_hardware_interfaces_test::kDrivetrainSettings);
+      panther_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10));
   }
 
   ~TestPantherRobotDriverInitialization() {}
@@ -96,21 +101,29 @@ class TestPantherRobotDriverInitialization : public ::testing::Test
   static constexpr char kLeftMotorDriverName[] = "left";
   static constexpr char kRightMotorDriverName[] = "right";
 
-  std::shared_ptr<MockDriver> front_driver_mock_;
-  std::shared_ptr<MockDriver> rear_driver_mock_;
+  std::shared_ptr<::testing::NiceMock<MockDriver>> front_driver_mock_;
+  std::shared_ptr<::testing::NiceMock<MockDriver>> rear_driver_mock_;
+  std::shared_ptr<::testing::NiceMock<MockMotorDriver>> fl_motor_driver_;
+  std::shared_ptr<::testing::NiceMock<MockMotorDriver>> fr_motor_driver_;
+  std::shared_ptr<::testing::NiceMock<MockMotorDriver>> rl_motor_driver_;
+  std::shared_ptr<::testing::NiceMock<MockMotorDriver>> rr_motor_driver_;
   std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
   std::unique_ptr<panther_hardware_interfaces::PantherRobotDriver> robot_driver_;
-  std::shared_ptr<MockMotorDriver> fl_motor_driver_;
-  std::shared_ptr<MockMotorDriver> fr_motor_driver_;
-  std::shared_ptr<MockMotorDriver> rl_motor_driver_;
-  std::shared_ptr<MockMotorDriver> rr_motor_driver_;
 };
 
 TEST_F(TestPantherRobotDriverInitialization, Initialize)
 {
+  std::cout << "TestPantherRobotDriverInitialization Initialize" << std::endl;
+  EXPECT_CALL(*front_driver_mock_, Boot()).Times(1);
+  EXPECT_CALL(*rear_driver_mock_, Boot()).Times(1);
+
+  std::cout << "TestPantherRobotDriverInitialization Initialize" << std::endl;
   ASSERT_NO_THROW(robot_driver_->Initialize());
   ASSERT_NO_THROW(robot_driver_->Deinitialize());
 
+  std::cout << "TestPantherRobotDriverInitialization Initialize" << std::endl;
+  EXPECT_CALL(*front_driver_mock_, Boot()).Times(1);
+  EXPECT_CALL(*rear_driver_mock_, Boot()).Times(1);
   // Check if deinitialization worked correctly - initialize once again
   ASSERT_NO_THROW(robot_driver_->Initialize());
   ASSERT_NO_THROW(robot_driver_->Deinitialize());
@@ -118,14 +131,12 @@ TEST_F(TestPantherRobotDriverInitialization, Initialize)
 
 TEST_F(TestPantherRobotDriverInitialization, Activate)
 {
-  EXPECT_CALL(*front_driver_mock_, Boot()).Times(1);
-  EXPECT_CALL(*rear_driver_mock_, Boot()).Times(1);
   EXPECT_CALL(*front_driver_mock_, ResetScript()).Times(1);
   EXPECT_CALL(*rear_driver_mock_, ResetScript()).Times(1);
-  EXPECT_CALL(*fl_motor_driver_, SendCmd(::testing::Eq(0))).Times(1);
-  EXPECT_CALL(*fr_motor_driver_, SendCmd(::testing::Eq(0))).Times(1);
-  EXPECT_CALL(*rl_motor_driver_, SendCmd(::testing::Eq(0))).Times(1);
-  EXPECT_CALL(*rr_motor_driver_, SendCmd(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*fl_motor_driver_, SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*fr_motor_driver_, SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*rl_motor_driver_, SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*rr_motor_driver_, SendCmdVel(::testing::Eq(0))).Times(1);
 
   ASSERT_NO_THROW(robot_driver_->Initialize());
   ASSERT_NO_THROW(robot_driver_->Activate());
@@ -158,7 +169,7 @@ TEST_F(TestPantherRobotDriver, UpdateCommunicationStateCANErorr)
   EXPECT_CALL(*front_driver_mock_, IsCANError()).WillOnce(::testing::Return(true));
   EXPECT_CALL(*rear_driver_mock_, IsCANError()).WillOnce(::testing::Return(true));
 
-  ASSERT_THROW(robot_driver_->UpdateCommunicationState(), std::runtime_error);
+  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
 
   EXPECT_TRUE(robot_driver_->GetData(kFrontDriverName).IsCANError());
   EXPECT_TRUE(robot_driver_->GetData(kRearDriverName).IsCANError());
@@ -169,7 +180,7 @@ TEST_F(TestPantherRobotDriver, UpdateCommunicationStateHeartbeatTimeout)
   EXPECT_CALL(*front_driver_mock_, IsHeartbeatTimeout()).WillOnce(::testing::Return(true));
   EXPECT_CALL(*rear_driver_mock_, IsHeartbeatTimeout()).WillOnce(::testing::Return(true));
 
-  ASSERT_THROW(robot_driver_->UpdateCommunicationState(), std::runtime_error);
+  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
 
   EXPECT_TRUE(robot_driver_->GetData(kFrontDriverName).IsHeartbeatTimeout());
   EXPECT_TRUE(robot_driver_->GetData(kRearDriverName).IsHeartbeatTimeout());
@@ -194,25 +205,25 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsState)
   const std::int32_t rr_vel = 402;
   const std::int32_t rr_current = 403;
 
-  ON_CALL(*fl_motor_driver_, ReadMotorState())
+  ON_CALL(*fl_motor_driver_, ReadMotorDriverState())
     .WillByDefault(::testing::Return(
       panther_hardware_interfaces::MotorDriverState({fl_pos, fl_vel, fl_current, {0, 0}, {0, 0}})));
-  ON_CALL(*fr_motor_driver_, ReadMotorState())
+  ON_CALL(*fr_motor_driver_, ReadMotorDriverState())
     .WillByDefault(::testing::Return(
       panther_hardware_interfaces::MotorDriverState({fr_pos, fr_vel, fr_current, {0, 0}, {0, 0}})));
-  ON_CALL(*rl_motor_driver_, ReadMotorState())
+  ON_CALL(*rl_motor_driver_, ReadMotorDriverState())
     .WillByDefault(::testing::Return(
       panther_hardware_interfaces::MotorDriverState({rl_pos, rl_vel, rl_current, {0, 0}, {0, 0}})));
-  ON_CALL(*rr_motor_driver_, ReadMotorState())
+  ON_CALL(*rr_motor_driver_, ReadMotorDriverState())
     .WillByDefault(::testing::Return(
       panther_hardware_interfaces::MotorDriverState({rr_pos, rr_vel, rr_current, {0, 0}, {0, 0}})));
 
   robot_driver_->UpdateMotorsState();
 
-  const auto & fl = robot_driver_->GetData(kFrontDriverName).GetMotorState(kLeftMotorDriverName);
-  const auto & fr = robot_driver_->GetData(kFrontDriverName).GetMotorState(kRightMotorDriverName);
-  const auto & rl = robot_driver_->GetData(kRearDriverName).GetMotorState(kLeftMotorDriverName);
-  const auto & rr = robot_driver_->GetData(kRearDriverName).GetMotorState(kRightMotorDriverName);
+  const auto & fl = robot_driver_->GetData(kFrontDriverName).GetLeftMotorState();
+  const auto & fr = robot_driver_->GetData(kFrontDriverName).GetRightMotorState();
+  const auto & rl = robot_driver_->GetData(kRearDriverName).GetLeftMotorState();
+  const auto & rr = robot_driver_->GetData(kRearDriverName).GetRightMotorState();
 
   EXPECT_FLOAT_EQ(fl.GetPosition(), fl_pos * kRbtqPosFbToRad);
   EXPECT_FLOAT_EQ(fl.GetVelocity(), fl_vel * kRbtqVelFbToRadPerSec);
@@ -233,8 +244,25 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsState)
 
 TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimestamps)
 {
+  auto read_motor_driver_state_method = []() {
+    panther_hardware_interfaces::MotorDriverState state;
+    clock_gettime(CLOCK_MONOTONIC, &state.pos_timestamp);
+    clock_gettime(CLOCK_MONOTONIC, &state.vel_current_timestamp);
+    return state;
+  };
+
+  ON_CALL(*fl_motor_driver_, ReadMotorDriverState())
+    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
+  ON_CALL(*fr_motor_driver_, ReadMotorDriverState())
+    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
+  ON_CALL(*rl_motor_driver_, ReadMotorDriverState())
+    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
+  ON_CALL(*rr_motor_driver_, ReadMotorDriverState())
+    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
+
   robot_driver_->UpdateMotorsState();
 
+  // sleep for timeout and check if timestamps were updated correctly
   std::this_thread::sleep_for(
     panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
     std::chrono::milliseconds(10));
@@ -250,11 +278,12 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimeout)
   timespec current_time;
   clock_gettime(CLOCK_MONOTONIC, &current_time);
 
-  panther_hardware_interfaces::MotorDriverState state;
-  state.pos_timestamp = current_time;
+  panther_hardware_interfaces::MotorDriverState state = {0, 0, 0, current_time, current_time};
 
-  EXPECT_CALL(*fl_motor_driver_, ReadMotorState()).WillOnce(::testing::Return(state));
-  EXPECT_CALL(*rl_motor_driver_, ReadMotorState()).WillOnce(::testing::Return(state));
+  ON_CALL(*fl_motor_driver_, ReadMotorDriverState()).WillByDefault(::testing::Return(state));
+  ON_CALL(*fr_motor_driver_, ReadMotorDriverState()).WillByDefault(::testing::Return(state));
+  ON_CALL(*rl_motor_driver_, ReadMotorDriverState()).WillByDefault(::testing::Return(state));
+  ON_CALL(*rr_motor_driver_, ReadMotorDriverState()).WillByDefault(::testing::Return(state));
 
   // sleep for pdo_motor_states_timeout_ms + 10ms
   std::this_thread::sleep_for(
@@ -271,10 +300,6 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimeout)
 
 TEST_F(TestPantherRobotDriver, UpdateDriverState)
 {
-  using panther_hardware_interfaces_test::DriverFaultFlags;
-  using panther_hardware_interfaces_test::DriverRuntimeErrors;
-  using panther_hardware_interfaces_test::DriverScriptFlags;
-
   const std::int16_t f_temp = 30;
   const std::int16_t r_temp = 32;
   const std::int16_t f_heatsink_temp = 31;
@@ -286,19 +311,19 @@ TEST_F(TestPantherRobotDriver, UpdateDriverState)
   const std::int16_t f_battery_current_2 = 30;
   const std::int16_t r_battery_current_2 = 40;
 
-  const std::uint8_t fault_flag_overheat = 0x01;
-  const std::uint8_t fault_flag_overvoltage = 0x02;
-  const std::uint8_t script_flag_amp_limiter = 0x01;
-  const std::uint8_t script_flag_encoder_disconnected = 0x02;
-  const std::uint8_t runtime_error_loop_error = 0x01;
-  const std::uint8_t runtime_error_safety_stop_active = 0x02;
-  const std::uint8_t runtime_error_forward_limit_triggered = 0x04;
-  const std::uint8_t runtime_error_reverse_limit_triggered = 0x08;
+  const std::uint8_t fault_flag_overheat = static_cast<std::uint8_t>(0b01);
+  const std::uint8_t fault_flag_overvoltage = static_cast<std::uint8_t>(0b10);
+  const std::uint8_t script_flag_encoder_disconnected = static_cast<std::uint8_t>(0b10);
+  const std::uint8_t script_flag_amp_limiter = static_cast<std::uint8_t>(0b100);
+  const std::uint8_t runtime_error_loop_error = static_cast<std::uint8_t>(0b100);
+  const std::uint8_t runtime_error_safety_stop_active = static_cast<std::uint8_t>(0b1000);
+  const std::uint8_t runtime_error_forward_limit_triggered = static_cast<std::uint8_t>(0b10000);
+  const std::uint8_t runtime_error_reverse_limit_triggered = static_cast<std::uint8_t>(0b100000);
 
   ON_CALL(*front_driver_mock_, ReadDriverState())
     .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
       {fault_flag_overheat,
-       script_flag_amp_limiter,
+       script_flag_encoder_disconnected,
        runtime_error_loop_error,
        runtime_error_safety_stop_active,
        f_battery_current_1,
@@ -311,7 +336,7 @@ TEST_F(TestPantherRobotDriver, UpdateDriverState)
   ON_CALL(*rear_driver_mock_, ReadDriverState())
     .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
       {fault_flag_overvoltage,
-       script_flag_encoder_disconnected,
+       script_flag_amp_limiter,
        runtime_error_forward_limit_triggered,
        runtime_error_reverse_limit_triggered,
        r_battery_current_1,
@@ -357,8 +382,21 @@ TEST_F(TestPantherRobotDriver, UpdateDriverState)
 
 TEST_F(TestPantherRobotDriver, UpdateDriverStateTimestamps)
 {
+  auto read_driver_state_method = []() {
+    panther_hardware_interfaces::DriverState state;
+    clock_gettime(CLOCK_MONOTONIC, &state.flags_current_timestamp);
+    clock_gettime(CLOCK_MONOTONIC, &state.voltages_temps_timestamp);
+    return state;
+  };
+
+  ON_CALL(*front_driver_mock_, ReadDriverState())
+    .WillByDefault(::testing::Invoke(read_driver_state_method));
+  ON_CALL(*rear_driver_mock_, ReadDriverState())
+    .WillByDefault(::testing::Invoke(read_driver_state_method));
+
   robot_driver_->UpdateDriversState();
 
+  // sleep for timeout and check if timestamps were updated correctly
   std::this_thread::sleep_for(
     panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
     std::chrono::milliseconds(10));
@@ -374,12 +412,13 @@ TEST_F(TestPantherRobotDriver, UpdateDriverStateTimeout)
   timespec current_time;
   clock_gettime(CLOCK_MONOTONIC, &current_time);
 
-  panther_hardware_interfaces::DriverState state;
-  state.flags_current_timestamp = current_time;
+  panther_hardware_interfaces::DriverState state = {
+    0, 0, 0, 0, 0, 0, 0, 0, 0, current_time, current_time};
 
-  EXPECT_CALL(*front_driver_mock_, ReadDriverState()).WillOnce(::testing::Return(state));
-  EXPECT_CALL(*rear_driver_mock_, ReadDriverState()).WillOnce(::testing::Return(state));
+  ON_CALL(*front_driver_mock_, ReadDriverState()).WillByDefault(::testing::Return(state));
+  ON_CALL(*rear_driver_mock_, ReadDriverState()).WillByDefault(::testing::Return(state));
 
+  // sleep for pdo_driver_state_timeout_ms + 10ms
   std::this_thread::sleep_for(
     panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
     std::chrono::milliseconds(10));
@@ -403,19 +442,19 @@ TEST_F(TestPantherRobotDriver, SendSpeedCommands)
 
   EXPECT_CALL(
     *fl_motor_driver_,
-    SendCmd(::testing::Eq(static_cast<std::int32_t>(fl_v * kRadPerSecToRbtqCmd))))
+    SendCmdVel(::testing::Eq(static_cast<std::int32_t>(fl_v * kRadPerSecToRbtqCmd))))
     .Times(1);
   EXPECT_CALL(
     *fr_motor_driver_,
-    SendCmd(::testing::Eq(static_cast<std::int32_t>(fr_v * kRadPerSecToRbtqCmd))))
+    SendCmdVel(::testing::Eq(static_cast<std::int32_t>(fr_v * kRadPerSecToRbtqCmd))))
     .Times(1);
   EXPECT_CALL(
     *rl_motor_driver_,
-    SendCmd(::testing::Eq(static_cast<std::int32_t>(rl_v * kRadPerSecToRbtqCmd))))
+    SendCmdVel(::testing::Eq(static_cast<std::int32_t>(rl_v * kRadPerSecToRbtqCmd))))
     .Times(1);
   EXPECT_CALL(
     *rr_motor_driver_,
-    SendCmd(::testing::Eq(static_cast<std::int32_t>(rr_v * kRadPerSecToRbtqCmd))))
+    SendCmdVel(::testing::Eq(static_cast<std::int32_t>(rr_v * kRadPerSecToRbtqCmd))))
     .Times(1);
 
   ASSERT_NO_THROW(robot_driver_->SendSpeedCommands(fl_v, fr_v, rl_v, rr_v));
@@ -457,24 +496,21 @@ TEST_F(TestPantherRobotDriver, TurnOffEStopError)
 
 TEST_F(TestPantherRobotDriver, SafetyStop)
 {
-  using panther_hardware_interfaces::PantherRobotDriver::kLeftChannel;
-  using panther_hardware_interfaces::PantherRobotDriver::kRightChannel;
-
-  EXPECT_CALL(*fl_motor_driver_, TurnOnSafetyStop(::testing::Eq(kLeftChannel))).Times(1);
-  EXPECT_CALL(*fr_motor_driver_, TurnOnSafetyStop(::testing::Eq(kRightChannel))).Times(1);
-  EXPECT_CALL(*rl_motor_driver_, TurnOnSafetyStop(::testing::Eq(kLeftChannel))).Times(1);
-  EXPECT_CALL(*rr_motor_driver_, TurnOnSafetyStop(::testing::Eq(kRightChannel))).Times(1);
+  EXPECT_CALL(*fl_motor_driver_, TurnOnSafetyStop()).Times(1);
+  EXPECT_CALL(*fr_motor_driver_, TurnOnSafetyStop()).Times(1);
+  EXPECT_CALL(*rl_motor_driver_, TurnOnSafetyStop()).Times(1);
+  EXPECT_CALL(*rr_motor_driver_, TurnOnSafetyStop()).Times(1);
 
   ASSERT_NO_THROW(robot_driver_->TurnOnSafetyStop());
 }
 
 TEST_F(TestPantherRobotDriver, SafetyStopError)
 {
-  EXPECT_CALL(*fl_motor_driver_, TurnOnSafetyStop(::testing::Eq(kLeftChannel)))
+  EXPECT_CALL(*fl_motor_driver_, TurnOnSafetyStop())
     .WillOnce(::testing::Throw(std::runtime_error("")));
-  EXPECT_CALL(*fr_motor_driver_, TurnOnSafetyStop(::testing::Eq(kRightChannel))).Times(0);
-  EXPECT_CALL(*rl_motor_driver_, TurnOnSafetyStop(::testing::Eq(kLeftChannel))).Times(0);
-  EXPECT_CALL(*rr_motor_driver_, TurnOnSafetyStop(::testing::Eq(kRightChannel))).Times(0);
+  EXPECT_CALL(*fr_motor_driver_, TurnOnSafetyStop()).Times(0);
+  EXPECT_CALL(*rl_motor_driver_, TurnOnSafetyStop()).Times(0);
+  EXPECT_CALL(*rr_motor_driver_, TurnOnSafetyStop()).Times(0);
 
   ASSERT_THROW(robot_driver_->TurnOnSafetyStop(), std::runtime_error);
 }
diff --git a/panther_hardware_interfaces/test/utils/test_constants.hpp b/panther_hardware_interfaces/test/utils/test_constants.hpp
index d7529b13..ffb00322 100644
--- a/panther_hardware_interfaces/test/utils/test_constants.hpp
+++ b/panther_hardware_interfaces/test/utils/test_constants.hpp
@@ -42,7 +42,7 @@ const panther_hardware_interfaces::DrivetrainSettings kDrivetrainSettings{
 
 constexpr float kRadPerSecToRbtqCmd = 30.08 * (1.0 / (2.0 * M_PI)) * 60.0 * (1000.0 / 3600.0);
 constexpr float kRbtqPosFbToRad = (1. / 1600) * (1.0 / 30.08) * (2.0 * M_PI);
-constexpr float kRbtqVelFbToRadPerSec = (1. / 30.08) * (1. / 60.) * (2.0 * M_PI);
+constexpr float kRbtqVelFbToRadPerSec = (3600. / 1000.) * (1. / 30.08) * (1. / 60.) * (2.0 * M_PI);
 constexpr float kRbtqCurrentFbToNewtonMeters = (1. / 10.) * 0.11 * 30.08 * 0.75;
 
 const std::string kPantherSystemName = "wheels";

From beb6010fb1591dbaf8ac0c796bfd7100ca074ea2 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Mon, 26 Aug 2024 14:05:06 +0000
Subject: [PATCH 021/100] update roboteq data converter

---
 .../panther_system/robot_driver/driver.hpp    |  4 +-
 .../robot_driver/robot_driver.hpp             |  8 +--
 .../robot_driver/roboteq_data_converters.hpp  | 57 +++++++++++++++----
 .../robot_driver/robot_driver.cpp             | 16 +++---
 .../robot_driver/roboteq_data_converters.cpp  | 26 ++++-----
 .../robot_driver/roboteq_driver.cpp           |  4 +-
 .../test_panther_robot_driver.cpp             | 24 +++++---
 .../robot_driver/test_roboteq_driver.cpp      |  4 +-
 8 files changed, 92 insertions(+), 51 deletions(-)

diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp
index 9a62e237..f8544a5d 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp
@@ -42,8 +42,8 @@ struct DriverState
 {
   std::uint8_t fault_flags;
   std::uint8_t script_flags;
-  std::uint8_t runtime_stat_flag_motor_1;
-  std::uint8_t runtime_stat_flag_motor_2;
+  std::uint8_t runtime_stat_flag_channel_1;
+  std::uint8_t runtime_stat_flag_channel_2;
 
   std::int16_t battery_current_1;
   std::int16_t battery_current_2;
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
index aeb65c2e..4db34f3c 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
@@ -36,10 +36,10 @@ struct PantherDriverNames
   static constexpr char REAR[] = "rear";
 };
 
-struct PantherMotorChannels
+struct PantherMotorChannel
 {
-  static constexpr std::uint8_t LEFT = 2;
-  static constexpr std::uint8_t RIGHT = 1;
+  static constexpr std::uint8_t LEFT = RoboteqDriver::kChannel2;
+  static constexpr std::uint8_t RIGHT = RoboteqDriver::kChannel1;
 };
 
 /**
@@ -256,7 +256,7 @@ class PantherRobotDriver : public RobotDriver
 
 private:
   void SetMotorsStates(
-    RoboteqData & data, const MotorDriverState & front_state, const MotorDriverState & rear_state,
+    RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
     const timespec & current_time);
   void SetDriverState(RoboteqData & data, const DriverState & state, const timespec & current_time);
 
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp
index cfb897c5..da90c9de 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp
@@ -185,12 +185,12 @@ class RoboteqData
 {
 public:
   RoboteqData(const DrivetrainSettings & drivetrain_settings)
-  : left_motor_state_(drivetrain_settings), right_motor_state_(drivetrain_settings)
+  : channel_1_motor_state_(drivetrain_settings), channel_2_motor_state_(drivetrain_settings)
   {
   }
 
   void SetMotorsStates(
-    const MotorDriverState & left_state, const MotorDriverState & right_state,
+    const MotorDriverState & channel_1_state, const MotorDriverState & channel_2_state,
     const bool data_timed_out);
   void SetDriverState(const DriverState & state, const bool data_timed_out);
   void SetCANError(const bool can_error) { can_error_ = can_error; }
@@ -198,8 +198,8 @@ class RoboteqData
 
   bool IsFlagError() const
   {
-    return fault_flags_.IsError() || script_flags_.IsError() || left_runtime_error_.IsError() ||
-           right_runtime_error_.IsError();
+    return fault_flags_.IsError() || script_flags_.IsError() ||
+           channel_1_runtime_error_.IsError() || channel_2_runtime_error_.IsError();
   }
 
   bool IsError() const
@@ -208,8 +208,25 @@ class RoboteqData
            can_error_ || heartbeat_timeout_;
   }
 
-  const MotorState & GetLeftMotorState() const { return left_motor_state_; }
-  const MotorState & GetRightMotorState() const { return right_motor_state_; }
+  /**
+   * @brief Returns motor state data for the given channel
+   *
+   * @param channel 1 or 2
+   * @return motor state data
+   * @throws std::runtime_error if invalid channel number
+   */
+  const MotorState & GetMotorState(const std::uint8_t channel) const
+  {
+    if (channel == RoboteqDriver::kChannel1) {
+      return channel_1_motor_state_;
+    } else if (channel == RoboteqDriver::kChannel2) {
+      return channel_2_motor_state_;
+    }
+
+    throw std::runtime_error("Invalid channel number");
+  }
+
+  const MotorState & GetRightMotorState() const { return channel_2_motor_state_; }
   const RoboteqDriverState & GetDriverState() const { return driver_state_; }
 
   bool IsMotorStatesDataTimedOut() const { return motor_states_data_timed_out_; }
@@ -219,8 +236,24 @@ class RoboteqData
 
   const FaultFlag & GetFaultFlag() const { return fault_flags_; }
   const ScriptFlag & GetScriptFlag() const { return script_flags_; }
-  const RuntimeError & GetLeftRuntimeError() const { return left_runtime_error_; }
-  const RuntimeError & GetRightRuntimeError() const { return right_runtime_error_; }
+
+  /**
+   * @brief Returns runtime error flags for the given channel
+   *
+   * @param channel 1 or 2
+   * @return runtime error flags
+   * @throws std::runtime_error if invalid channel number
+   */
+  const RuntimeError & GetRuntimeError(const std::uint8_t channel) const
+  {
+    if (channel == RoboteqDriver::kChannel1) {
+      return channel_1_runtime_error_;
+    } else if (channel == RoboteqDriver::kChannel2) {
+      return channel_2_runtime_error_;
+    }
+
+    throw std::runtime_error("Invalid channel number");
+  }
 
   std::string GetFlagErrorLog() const;
 
@@ -228,15 +261,15 @@ class RoboteqData
   std::map<std::string, bool> GetErrorMap() const;
 
 private:
-  MotorState left_motor_state_;
-  MotorState right_motor_state_;
+  MotorState channel_1_motor_state_;
+  MotorState channel_2_motor_state_;
 
   RoboteqDriverState driver_state_;
 
   FaultFlag fault_flags_;
   ScriptFlag script_flags_;
-  RuntimeError left_runtime_error_;
-  RuntimeError right_runtime_error_;
+  RuntimeError channel_1_runtime_error_;
+  RuntimeError channel_2_runtime_error_;
 
   bool motor_states_data_timed_out_ = false;
   bool driver_state_data_timed_out_ = false;
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/robot_driver.cpp
index 1fb31adc..d1e4cac9 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/robot_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/robot_driver.cpp
@@ -257,19 +257,19 @@ void PantherRobotDriver::TurnOnSafetyStop()
 }
 
 void PantherRobotDriver::SetMotorsStates(
-  RoboteqData & data, const MotorDriverState & front_state, const MotorDriverState & rear_state,
+  RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
   const timespec & current_time)
 {
   // TODO figure out both motors timestamps
-  bool data_timed_out = (lely::util::from_timespec(current_time) -
-                           lely::util::from_timespec(front_state.pos_timestamp) >
-                         pdo_motor_states_timeout_ms_) ||
-                        (lely::util::from_timespec(current_time) -
-                           lely::util::from_timespec(front_state.vel_current_timestamp) >
-                         pdo_motor_states_timeout_ms_);
+  bool data_timed_out =
+    (lely::util::from_timespec(current_time) - lely::util::from_timespec(left_state.pos_timestamp) >
+     pdo_motor_states_timeout_ms_) ||
+    (lely::util::from_timespec(current_time) -
+       lely::util::from_timespec(left_state.vel_current_timestamp) >
+     pdo_motor_states_timeout_ms_);
 
   // Channel 1 - right, Channel 2 - left
-  data.SetMotorsStates(front_state, rear_state, data_timed_out);
+  data.SetMotorsStates(right_state, left_state, data_timed_out);
 }
 
 void PantherRobotDriver::SetDriverState(
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp
index a025a3f1..888a87b9 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp
@@ -203,11 +203,11 @@ std::map<std::string, bool> RuntimeError::GetErrorMap() const
 }
 
 void RoboteqData::SetMotorsStates(
-  const MotorDriverState & left_state, const MotorDriverState & right_state,
+  const MotorDriverState & channel_1_state, const MotorDriverState & channel_2_state,
   const bool data_timed_out)
 {
-  left_motor_state_.SetData(left_state);
-  right_motor_state_.SetData(right_state);
+  channel_1_motor_state_.SetData(channel_1_state);
+  channel_2_motor_state_.SetData(channel_2_state);
   motor_states_data_timed_out_ = data_timed_out;
 }
 
@@ -221,8 +221,8 @@ void RoboteqData::SetDriverState(const DriverState & state, const bool data_time
 
   fault_flags_.SetData(state.fault_flags);
   script_flags_.SetData(state.script_flags);
-  left_runtime_error_.SetData(state.runtime_stat_flag_motor_2);
-  right_runtime_error_.SetData(state.runtime_stat_flag_motor_1);
+  channel_1_runtime_error_.SetData(state.runtime_stat_flag_channel_1);
+  channel_2_runtime_error_.SetData(state.runtime_stat_flag_channel_2);
 
   driver_state_data_timed_out_ = data_timed_out;
 }
@@ -231,8 +231,8 @@ std::string RoboteqData::GetFlagErrorLog() const
 {
   return "Fault flags: " + fault_flags_.GetErrorLog() +
          "Script flags: " + script_flags_.GetErrorLog() +
-         "Left motor runtime flags: " + left_runtime_error_.GetErrorLog() +
-         "Right motor runtime flags: " + right_runtime_error_.GetErrorLog();
+         "Channel 1 motor runtime flags: " + channel_1_runtime_error_.GetErrorLog() +
+         "Channel 2 motor runtime flags: " + channel_2_runtime_error_.GetErrorLog();
 }
 
 std::map<std::string, bool> RoboteqData::GetFlagErrorMap() const
@@ -242,14 +242,14 @@ std::map<std::string, bool> RoboteqData::GetFlagErrorMap() const
   flag_error_map.merge(fault_flags_.GetErrorMap());
   flag_error_map.merge(script_flags_.GetErrorMap());
 
-  auto left_runtime_error_map = panther_utils::common_utilities::PrefixMapKeys(
-    left_runtime_error_.GetErrorMap(), "left_motor.");
+  auto channel_1_runtime_error_map = panther_utils::common_utilities::PrefixMapKeys(
+    channel_1_runtime_error_.GetErrorMap(), "channel_1_motor.");
 
-  auto right_runtime_error_map = panther_utils::common_utilities::PrefixMapKeys(
-    right_runtime_error_.GetErrorMap(), "right_motor.");
+  auto channel_2_runtime_error_map = panther_utils::common_utilities::PrefixMapKeys(
+    channel_2_runtime_error_.GetErrorMap(), "channel_2_motor.");
 
-  flag_error_map.merge(std::move(left_runtime_error_map));
-  flag_error_map.merge(std::move(right_runtime_error_map));
+  flag_error_map.merge(std::move(channel_1_runtime_error_map));
+  flag_error_map.merge(std::move(channel_2_runtime_error_map));
 
   return flag_error_map;
 }
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp
index c2c8d853..68c5e114 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp
@@ -124,8 +124,8 @@ DriverState RoboteqDriver::ReadDriverState()
   std::int32_t flags = static_cast<std::int32_t>(
     rpdo_mapped[RoboteqCANObjects::flags.id][RoboteqCANObjects::flags.subid]);
   state.fault_flags = GetByte(flags, 0);
-  state.runtime_stat_flag_motor_1 = GetByte(flags, 1);
-  state.runtime_stat_flag_motor_2 = GetByte(flags, 2);
+  state.runtime_stat_flag_channel_1 = GetByte(flags, 1);
+  state.runtime_stat_flag_channel_2 = GetByte(flags, 2);
   state.script_flags = GetByte(flags, 3);
 
   state.mcu_temp = rpdo_mapped[RoboteqCANObjects::mcu_temp.id][RoboteqCANObjects::mcu_temp.subid];
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
index 7a795ff7..2d513579 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
@@ -188,6 +188,7 @@ TEST_F(TestPantherRobotDriver, UpdateCommunicationStateHeartbeatTimeout)
 
 TEST_F(TestPantherRobotDriver, UpdateMotorsState)
 {
+  using panther_hardware_interfaces::PantherMotorChannel;
   using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
   using panther_hardware_interfaces_test::kRbtqPosFbToRad;
   using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
@@ -220,10 +221,14 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsState)
 
   robot_driver_->UpdateMotorsState();
 
-  const auto & fl = robot_driver_->GetData(kFrontDriverName).GetLeftMotorState();
-  const auto & fr = robot_driver_->GetData(kFrontDriverName).GetRightMotorState();
-  const auto & rl = robot_driver_->GetData(kRearDriverName).GetLeftMotorState();
-  const auto & rr = robot_driver_->GetData(kRearDriverName).GetRightMotorState();
+  const auto & fl =
+    robot_driver_->GetData(kFrontDriverName).GetMotorState(PantherMotorChannel::LEFT);
+  const auto & fr =
+    robot_driver_->GetData(kFrontDriverName).GetMotorState(PantherMotorChannel::RIGHT);
+  const auto & rl =
+    robot_driver_->GetData(kRearDriverName).GetMotorState(PantherMotorChannel::LEFT);
+  const auto & rr =
+    robot_driver_->GetData(kRearDriverName).GetMotorState(PantherMotorChannel::RIGHT);
 
   EXPECT_FLOAT_EQ(fl.GetPosition(), fl_pos * kRbtqPosFbToRad);
   EXPECT_FLOAT_EQ(fl.GetVelocity(), fl_vel * kRbtqVelFbToRadPerSec);
@@ -300,6 +305,8 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimeout)
 
 TEST_F(TestPantherRobotDriver, UpdateDriverState)
 {
+  using panther_hardware_interfaces::PantherMotorChannel;
+
   const std::int16_t f_temp = 30;
   const std::int16_t r_temp = 32;
   const std::int16_t f_heatsink_temp = 31;
@@ -371,13 +378,14 @@ TEST_F(TestPantherRobotDriver, UpdateDriverState)
 
   EXPECT_TRUE(front.GetFaultFlag().GetMessage().overheat);
   EXPECT_TRUE(front.GetScriptFlag().GetMessage().encoder_disconnected);
-  EXPECT_TRUE(front.GetRightRuntimeError().GetMessage().loop_error);
-  EXPECT_TRUE(front.GetLeftRuntimeError().GetMessage().safety_stop_active);
+  EXPECT_TRUE(front.GetRuntimeError(PantherMotorChannel::RIGHT).GetMessage().loop_error);
+  EXPECT_TRUE(front.GetRuntimeError(PantherMotorChannel::LEFT).GetMessage().safety_stop_active);
 
   EXPECT_TRUE(rear.GetFaultFlag().GetMessage().overvoltage);
   EXPECT_TRUE(rear.GetScriptFlag().GetMessage().amp_limiter);
-  EXPECT_TRUE(rear.GetRightRuntimeError().GetMessage().forward_limit_triggered);
-  EXPECT_TRUE(rear.GetLeftRuntimeError().GetMessage().reverse_limit_triggered);
+  EXPECT_TRUE(
+    rear.GetRuntimeError(PantherMotorChannel::RIGHT).GetMessage().forward_limit_triggered);
+  EXPECT_TRUE(rear.GetRuntimeError(PantherMotorChannel::LEFT).GetMessage().reverse_limit_triggered);
 }
 
 TEST_F(TestPantherRobotDriver, UpdateDriverStateTimestamps)
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
index b95dafb3..8da958b4 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
@@ -241,8 +241,8 @@ TEST_F(TestRoboteqDriver, ReadDriverState)
 
   EXPECT_EQ(fb.fault_flags, 0b00000001);
   EXPECT_EQ(fb.script_flags, 0b00000010);
-  EXPECT_EQ(fb.runtime_stat_flag_motor_1, 0b00000100);
-  EXPECT_EQ(fb.runtime_stat_flag_motor_2, 0b00001000);
+  EXPECT_EQ(fb.runtime_stat_flag_channel_1, 0b00000100);
+  EXPECT_EQ(fb.runtime_stat_flag_channel_2, 0b00001000);
 }
 
 TEST_F(TestRoboteqDriver, ReadDriverStateTimestamp)

From 3e7ce380d52d602b84b1adfaff9ab4e6f21eb7da Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Tue, 27 Aug 2024 06:55:13 +0000
Subject: [PATCH 022/100] move data converters and error filter tests

---
 panther_hardware_interfaces/CMakeLists.txt    | 13 ++--
 .../robot_driver/roboteq_data_converters.hpp  | 23 +-----
 .../robot_driver/roboteq_data_converters.cpp  | 22 ++++++
 .../test_roboteq_data_converters.cpp          | 75 +++++++++++--------
 .../test_roboteq_error_filter.cpp             |  2 +-
 5 files changed, 73 insertions(+), 62 deletions(-)
 rename panther_hardware_interfaces/test/{panther_system/motors_controller => unit/panther_system/robot_driver}/test_roboteq_data_converters.cpp (78%)
 rename panther_hardware_interfaces/test/{panther_system/motors_controller => unit/panther_system/robot_driver}/test_roboteq_error_filter.cpp (99%)

diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt
index fd997127..a8cd31b0 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/panther_hardware_interfaces/CMakeLists.txt
@@ -84,6 +84,7 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_gmock REQUIRED)
   find_package(panther_utils REQUIRED)
 
   install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)
@@ -100,12 +101,12 @@ if(BUILD_TESTING)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_error_filter
-    test/panther_system/robot_driver/test_roboteq_error_filter.cpp
+    test/unit/panther_system/robot_driver/test_roboteq_error_filter.cpp
     src/panther_system/robot_driver/roboteq_error_filter.cpp)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_data_converters
-    test/panther_system/robot_driver/test_roboteq_data_converters.cpp
+    test/unit/panther_system/robot_driver/test_roboteq_data_converters.cpp
     src/panther_system/robot_driver/roboteq_data_converters.cpp src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_roboteq_data_converters
@@ -119,9 +120,7 @@ if(BUILD_TESTING)
   ament_add_gtest(
     ${PROJECT_NAME}_test_canopen_manager
     test/unit/panther_system/robot_driver/test_canopen_manager.cpp
-    src/panther_system/robot_driver/canopen_manager.cpp
-    src/panther_system/robot_driver/roboteq_driver.cpp
-    src/utils.cpp)
+    src/panther_system/robot_driver/canopen_manager.cpp src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_canopen_manager
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
@@ -146,9 +145,9 @@ if(BUILD_TESTING)
   target_link_libraries(${PROJECT_NAME}_test_roboteq_driver
                         PkgConfig::LIBLELY_COAPP)
 
-  ament_add_gtest(
+  ament_add_gmock(
     ${PROJECT_NAME}_test_robot_driver
-    test/panther_system/robot_driver/test_robot_driver.cpp
+    test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
     src/panther_system/robot_driver/canopen_manager.cpp
     src/panther_system/robot_driver/roboteq_driver.cpp
     src/panther_system/robot_driver/roboteq_data_converters.cpp
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp
index da90c9de..06a3af84 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp
@@ -215,18 +215,8 @@ class RoboteqData
    * @return motor state data
    * @throws std::runtime_error if invalid channel number
    */
-  const MotorState & GetMotorState(const std::uint8_t channel) const
-  {
-    if (channel == RoboteqDriver::kChannel1) {
-      return channel_1_motor_state_;
-    } else if (channel == RoboteqDriver::kChannel2) {
-      return channel_2_motor_state_;
-    }
-
-    throw std::runtime_error("Invalid channel number");
-  }
+  const MotorState & GetMotorState(const std::uint8_t channel) const;
 
-  const MotorState & GetRightMotorState() const { return channel_2_motor_state_; }
   const RoboteqDriverState & GetDriverState() const { return driver_state_; }
 
   bool IsMotorStatesDataTimedOut() const { return motor_states_data_timed_out_; }
@@ -244,16 +234,7 @@ class RoboteqData
    * @return runtime error flags
    * @throws std::runtime_error if invalid channel number
    */
-  const RuntimeError & GetRuntimeError(const std::uint8_t channel) const
-  {
-    if (channel == RoboteqDriver::kChannel1) {
-      return channel_1_runtime_error_;
-    } else if (channel == RoboteqDriver::kChannel2) {
-      return channel_2_runtime_error_;
-    }
-
-    throw std::runtime_error("Invalid channel number");
-  }
+  const RuntimeError & GetRuntimeError(const std::uint8_t channel) const;
 
   std::string GetFlagErrorLog() const;
 
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp
index 888a87b9..283ebe09 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp
@@ -227,6 +227,28 @@ void RoboteqData::SetDriverState(const DriverState & state, const bool data_time
   driver_state_data_timed_out_ = data_timed_out;
 }
 
+const MotorState & RoboteqData::GetMotorState(const std::uint8_t channel) const
+{
+  if (channel == RoboteqDriver::kChannel1) {
+    return channel_1_motor_state_;
+  } else if (channel == RoboteqDriver::kChannel2) {
+    return channel_2_motor_state_;
+  }
+
+  throw std::runtime_error("Invalid channel number");
+}
+
+const RuntimeError & RoboteqData::GetRuntimeError(const std::uint8_t channel) const
+{
+  if (channel == RoboteqDriver::kChannel1) {
+    return channel_1_runtime_error_;
+  } else if (channel == RoboteqDriver::kChannel2) {
+    return channel_2_runtime_error_;
+  }
+
+  throw std::runtime_error("Invalid channel number");
+}
+
 std::string RoboteqData::GetFlagErrorLog() const
 {
   return "Fault flags: " + fault_flags_.GetErrorLog() +
diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_data_converters.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_data_converters.cpp
similarity index 78%
rename from panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_data_converters.cpp
rename to panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_data_converters.cpp
index c7018a1f..8780282e 100644
--- a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_data_converters.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_data_converters.cpp
@@ -19,7 +19,8 @@
 
 #include <gtest/gtest.h>
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
+
 #include "utils/test_constants.hpp"
 
 void TestFaultFlagMsg(
@@ -83,6 +84,10 @@ TEST(TestRoboteqDataConverters, CommandConverter)
 
 TEST(TestRoboteqDataConverters, MotorState)
 {
+  using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  using panther_hardware_interfaces_test::kRbtqPosFbToRad;
+  using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+
   panther_hardware_interfaces::MotorState motor_state(
     panther_hardware_interfaces_test::kDrivetrainSettings);
 
@@ -90,29 +95,25 @@ TEST(TestRoboteqDataConverters, MotorState)
   EXPECT_FLOAT_EQ(motor_state.GetVelocity(), 0.0);
   EXPECT_FLOAT_EQ(motor_state.GetTorque(), 0.0);
 
-  panther_hardware_interfaces::RoboteqMotorState feedback1;
+  panther_hardware_interfaces::MotorDriverState feedback1;
   feedback1.pos = 48128;
   feedback1.vel = 1000;
   feedback1.current = 1;
   motor_state.SetData(feedback1);
 
-  const float pos_to_radians = 0.00013055156;
-  const float vel_to_radians_per_second = 0.003481375;
-  const float current_to_newton_meters = 0.24816;
+  EXPECT_FLOAT_EQ(motor_state.GetPosition(), feedback1.pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(motor_state.GetVelocity(), feedback1.vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(motor_state.GetTorque(), feedback1.current * kRbtqCurrentFbToNewtonMeters);
 
-  EXPECT_FLOAT_EQ(motor_state.GetPosition(), feedback1.pos * pos_to_radians);
-  EXPECT_FLOAT_EQ(motor_state.GetVelocity(), feedback1.vel * vel_to_radians_per_second);
-  EXPECT_FLOAT_EQ(motor_state.GetTorque(), feedback1.current * current_to_newton_meters);
-
-  panther_hardware_interfaces::RoboteqMotorState feedback2;
+  panther_hardware_interfaces::MotorDriverState feedback2;
   feedback2.pos = -48128;
   feedback2.vel = -1000;
   feedback2.current = -1;
   motor_state.SetData(feedback2);
 
-  EXPECT_FLOAT_EQ(motor_state.GetPosition(), feedback2.pos * pos_to_radians);
-  EXPECT_FLOAT_EQ(motor_state.GetVelocity(), feedback2.vel * vel_to_radians_per_second);
-  EXPECT_FLOAT_EQ(motor_state.GetTorque(), feedback2.current * current_to_newton_meters);
+  EXPECT_FLOAT_EQ(motor_state.GetPosition(), feedback2.pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(motor_state.GetVelocity(), feedback2.vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(motor_state.GetTorque(), feedback2.current * kRbtqCurrentFbToNewtonMeters);
 }
 
 TEST(TestRoboteqDataConverters, FlagError)
@@ -214,7 +215,7 @@ TEST(TestRoboteqDataConverters, RuntimeError)
 
 TEST(TestRoboteqDataConverters, DriverState)
 {
-  panther_hardware_interfaces::DriverState driver_state;
+  panther_hardware_interfaces::RoboteqDriverState driver_state;
 
   EXPECT_FLOAT_EQ(driver_state.GetTemperature(), 0.0);
   EXPECT_FLOAT_EQ(driver_state.GetVoltage(), 0.0);
@@ -232,19 +233,26 @@ TEST(TestRoboteqDataConverters, DriverState)
 
 TEST(TestRoboteqDataConverters, RoboteqData)
 {
+  using panther_hardware_interfaces::RoboteqDriver;
+  using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  using panther_hardware_interfaces_test::kRbtqPosFbToRad;
+  using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+
   panther_hardware_interfaces::RoboteqData roboteq_data(
     panther_hardware_interfaces_test::kDrivetrainSettings);
 
   ASSERT_FALSE(roboteq_data.IsError());
 
-  const panther_hardware_interfaces::RoboteqMotorState left_state = {48128, 1000, 1};
-  const panther_hardware_interfaces::RoboteqMotorState right_state = {0, 0, 0};
-  roboteq_data.SetMotorsStates(left_state, right_state, true);
+  const panther_hardware_interfaces::MotorDriverState channel_1_state = {0, 0, 0, {0, 0}, {0, 0}};
+  const panther_hardware_interfaces::MotorDriverState channel_2_state = {
+    48128, 1000, 1, {0, 0}, {0, 0}};
+
+  roboteq_data.SetMotorsStates(channel_1_state, channel_2_state, true);
 
   ASSERT_TRUE(roboteq_data.IsError());
   ASSERT_TRUE(roboteq_data.IsMotorStatesDataTimedOut());
 
-  roboteq_data.SetMotorsStates(left_state, right_state, false);
+  roboteq_data.SetMotorsStates(channel_1_state, channel_2_state, false);
 
   ASSERT_FALSE(roboteq_data.IsError());
 
@@ -256,25 +264,25 @@ TEST(TestRoboteqDataConverters, RoboteqData)
   roboteq_data.SetCANError(false);
   ASSERT_FALSE(roboteq_data.IsError());
 
-  const float pos_to_radians = 0.00013055156;
-  const float vel_to_radians_per_second = 0.003481375;
-  const float current_to_newton_meters = 0.24816;
-
-  EXPECT_FLOAT_EQ(roboteq_data.GetRightMotorState().GetPosition(), 0.0);
-  EXPECT_FLOAT_EQ(roboteq_data.GetRightMotorState().GetVelocity(), 0.0);
-  EXPECT_FLOAT_EQ(roboteq_data.GetRightMotorState().GetTorque(), 0.0);
-  EXPECT_FLOAT_EQ(roboteq_data.GetLeftMotorState().GetPosition(), left_state.pos * pos_to_radians);
+  EXPECT_FLOAT_EQ(roboteq_data.GetMotorState(RoboteqDriver::kChannel1).GetPosition(), 0.0);
+  EXPECT_FLOAT_EQ(roboteq_data.GetMotorState(RoboteqDriver::kChannel1).GetVelocity(), 0.0);
+  EXPECT_FLOAT_EQ(roboteq_data.GetMotorState(RoboteqDriver::kChannel1).GetTorque(), 0.0);
+  EXPECT_FLOAT_EQ(
+    roboteq_data.GetMotorState(RoboteqDriver::kChannel2).GetPosition(),
+    channel_2_state.pos * kRbtqPosFbToRad);
   EXPECT_FLOAT_EQ(
-    roboteq_data.GetLeftMotorState().GetVelocity(), left_state.vel * vel_to_radians_per_second);
+    roboteq_data.GetMotorState(RoboteqDriver::kChannel2).GetVelocity(),
+    channel_2_state.vel * kRbtqVelFbToRadPerSec);
   EXPECT_FLOAT_EQ(
-    roboteq_data.GetLeftMotorState().GetTorque(), left_state.current * current_to_newton_meters);
+    roboteq_data.GetMotorState(RoboteqDriver::kChannel2).GetTorque(),
+    channel_2_state.current * kRbtqCurrentFbToNewtonMeters);
 
-  panther_hardware_interfaces::RoboteqDriverState state;
+  panther_hardware_interfaces::DriverState state;
 
   state.fault_flags = 0b00000001;
   state.script_flags = 0b00000010;
-  state.runtime_stat_flag_motor_1 = 0b00010000;
-  state.runtime_stat_flag_motor_2 = 0b00000100;
+  state.runtime_stat_flag_channel_1 = 0b00010000;
+  state.runtime_stat_flag_channel_2 = 0b00000100;
 
   state.mcu_temp = 32;
   state.heatsink_temp = 31;
@@ -288,8 +296,9 @@ TEST(TestRoboteqDataConverters, RoboteqData)
 
   EXPECT_TRUE(roboteq_data.GetFaultFlag().GetMessage().overheat);
   EXPECT_TRUE(roboteq_data.GetScriptFlag().GetMessage().encoder_disconnected);
-  EXPECT_TRUE(roboteq_data.GetLeftRuntimeError().GetMessage().loop_error);
-  EXPECT_TRUE(roboteq_data.GetRightRuntimeError().GetMessage().forward_limit_triggered);
+  EXPECT_TRUE(roboteq_data.GetRuntimeError(RoboteqDriver::kChannel2).GetMessage().loop_error);
+  EXPECT_TRUE(
+    roboteq_data.GetRuntimeError(RoboteqDriver::kChannel1).GetMessage().forward_limit_triggered);
 
   EXPECT_FLOAT_EQ(roboteq_data.GetDriverState().GetTemperature(), 32);
   EXPECT_FLOAT_EQ(roboteq_data.GetDriverState().GetHeatsinkTemperature(), 31);
diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_error_filter.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_error_filter.cpp
similarity index 99%
rename from panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_error_filter.cpp
rename to panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_error_filter.cpp
index 999243f6..cd3d6566 100644
--- a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_error_filter.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_error_filter.cpp
@@ -14,7 +14,7 @@
 
 #include <gtest/gtest.h>
 
-#include <panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp>
+#include <panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp>
 
 TEST(TestRoboteqErrorFilter, InitialState)
 {

From fd3182d61ed954e3745f1b9f168101f8845be226 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Tue, 27 Aug 2024 10:53:53 +0000
Subject: [PATCH 023/100] remove bad prints

---
 .../panther_system/robot_driver/test_panther_robot_driver.cpp  | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
index 2d513579..30a8aa3d 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
@@ -113,15 +113,12 @@ class TestPantherRobotDriverInitialization : public ::testing::Test
 
 TEST_F(TestPantherRobotDriverInitialization, Initialize)
 {
-  std::cout << "TestPantherRobotDriverInitialization Initialize" << std::endl;
   EXPECT_CALL(*front_driver_mock_, Boot()).Times(1);
   EXPECT_CALL(*rear_driver_mock_, Boot()).Times(1);
 
-  std::cout << "TestPantherRobotDriverInitialization Initialize" << std::endl;
   ASSERT_NO_THROW(robot_driver_->Initialize());
   ASSERT_NO_THROW(robot_driver_->Deinitialize());
 
-  std::cout << "TestPantherRobotDriverInitialization Initialize" << std::endl;
   EXPECT_CALL(*front_driver_mock_, Boot()).Times(1);
   EXPECT_CALL(*rear_driver_mock_, Boot()).Times(1);
   // Check if deinitialization worked correctly - initialize once again

From 36d1ebda34197255b331dc611741dc42a6a89f31 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Tue, 27 Aug 2024 13:36:48 +0000
Subject: [PATCH 024/100] review fixes

---
 .../motors_controller/canopen_manager.hpp     |   6 +
 .../motors_controller/roboteq_driver.cpp      |   7 +-
 .../test/config/canopen_configuration.yaml    |   5 -
 .../test/config/slave_2.bin                   | Bin 13 -> 0 bytes
 .../motors_controller/test_roboteq_driver.cpp | 111 +++++++++---------
 .../{roboteqs_mock.hpp => roboteq_mock.hpp}   |  15 +--
 6 files changed, 67 insertions(+), 77 deletions(-)
 delete mode 100644 panther_hardware_interfaces/test/config/slave_2.bin
 rename panther_hardware_interfaces/test/utils/{roboteqs_mock.hpp => roboteq_mock.hpp} (97%)

diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp
index fb9721c7..195fe11d 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp
@@ -36,6 +36,12 @@
 namespace panther_hardware_interfaces
 {
 
+struct CANopenObject
+{
+  const std::uint16_t id;
+  const std::uint8_t subid;
+};
+
 struct CANopenSettings
 {
   std::string can_interface_name;
diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp b/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp
index d563a495..93340174 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp
@@ -24,17 +24,12 @@
 #include <string>
 #include <system_error>
 
+#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp"
 #include "panther_hardware_interfaces/utils.hpp"
 
 namespace panther_hardware_interfaces
 {
 
-struct CANopenObject
-{
-  const std::uint16_t id;
-  const std::uint8_t subid;
-};
-
 // All ids and sub ids were read directly from the eds file. Lely CANopen doesn't have the option
 // to parse them based on the ParameterName. Additionally between version v60 and v80
 // ParameterName changed, for example: Cmd_ESTOP (old), Cmd_ESTOP Emergency Shutdown (new) As
diff --git a/panther_hardware_interfaces/test/config/canopen_configuration.yaml b/panther_hardware_interfaces/test/config/canopen_configuration.yaml
index 8f0a70a2..bef2f4cd 100644
--- a/panther_hardware_interfaces/test/config/canopen_configuration.yaml
+++ b/panther_hardware_interfaces/test/config/canopen_configuration.yaml
@@ -6,8 +6,3 @@ slave_1:
   dcf: roboteq_motor_controllers_v80_21a.eds
   node_id: 1
   heartbeat_producer: 100
-
-slave_2:
-  dcf: roboteq_motor_controllers_v80_21a.eds
-  node_id: 2
-  heartbeat_producer: 100
diff --git a/panther_hardware_interfaces/test/config/slave_2.bin b/panther_hardware_interfaces/test/config/slave_2.bin
deleted file mode 100644
index c042c65d87474cff1e582580cc27e9c59c20250e..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 13
ScmZQ%U|<jzU|<4LDGUGtJphjY

diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
index 022662bf..5edd9f97 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
@@ -24,7 +24,7 @@
 #include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp"
 
 #include "utils/fake_can_socket.hpp"
-#include "utils/roboteqs_mock.hpp"
+#include "utils/roboteq_mock.hpp"
 #include "utils/test_constants.hpp"
 
 #include "panther_utils/test/test_utils.hpp"
@@ -45,7 +45,7 @@ class TestRoboteqDriver : public ::testing::Test
   static constexpr char kMotor2Name[] = "motor_2";
 
   std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
-  std::unique_ptr<panther_hardware_interfaces_test::RoboteqsMock> roboteqs_mock_;
+  std::unique_ptr<panther_hardware_interfaces_test::RoboteqMock> roboteq_mock_;
   std::unique_ptr<panther_hardware_interfaces::CANopenManager> canopen_manager_;
   std::shared_ptr<panther_hardware_interfaces::RoboteqDriver> roboteq_driver_;
 };
@@ -59,8 +59,8 @@ TestRoboteqDriver::TestRoboteqDriver()
   canopen_manager_ = std::make_unique<panther_hardware_interfaces::CANopenManager>(
     panther_hardware_interfaces_test::kCANopenSettings);
 
-  roboteqs_mock_ = std::make_unique<panther_hardware_interfaces_test::RoboteqsMock>();
-  roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
+  roboteq_mock_ = std::make_unique<panther_hardware_interfaces_test::RoboteqMock>();
+  roboteq_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
   canopen_manager_->Initialize();
 
   roboteq_driver_ = std::make_shared<panther_hardware_interfaces::RoboteqDriver>(
@@ -71,8 +71,8 @@ TestRoboteqDriver::~TestRoboteqDriver()
 {
   roboteq_driver_.reset();
   canopen_manager_->Deinitialize();
-  roboteqs_mock_->Stop();
-  roboteqs_mock_.reset();
+  roboteq_mock_->Stop();
+  roboteq_mock_.reset();
   can_socket_->Deinitialize();
 }
 
@@ -111,22 +111,19 @@ TEST_F(TestRoboteqDriver, BootRoboteqDriver) { ASSERT_TRUE(TestBoot()); }
 
 TEST_F(TestRoboteqDriver, BootErrorDeviceType)
 {
-  roboteqs_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(0x1000, 0, 100000);
-  ASSERT_FALSE(TestBoot());
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(50));
-  roboteqs_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(0x1000, 0, 0);
-  ASSERT_TRUE(TestBoot());
+  const auto device_type_id = 0x1000;
+  const auto device_type_subid = 0;
+  roboteq_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(
+    device_type_id, device_type_subid, 100000);
+  EXPECT_FALSE(TestBoot());
 }
 
 TEST_F(TestRoboteqDriver, BootErrorVendorId)
 {
-  roboteqs_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(0x1018, 1, 100000);
-  ASSERT_FALSE(TestBoot());
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(50));
-  roboteqs_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(0x1018, 1, 0);
-  ASSERT_TRUE(TestBoot());
+  const auto vendor_id_id = 0x1018;
+  const auto vendor_id_subid = 1;
+  roboteq_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(vendor_id_id, vendor_id_subid, 100000);
+  EXPECT_FALSE(TestBoot());
 }
 
 TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
@@ -142,16 +139,16 @@ TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
 
   BootRoboteqDriver();
 
-  roboteqs_mock_->GetDriver()->SetPosition(DriverChannel::CHANNEL1, motor_1_pos);
-  roboteqs_mock_->GetDriver()->SetPosition(DriverChannel::CHANNEL2, motor_2_pos);
+  roboteq_mock_->GetDriver()->SetPosition(DriverChannel::CHANNEL1, motor_1_pos);
+  roboteq_mock_->GetDriver()->SetPosition(DriverChannel::CHANNEL2, motor_2_pos);
 
-  roboteqs_mock_->GetDriver()->SetVelocity(DriverChannel::CHANNEL1, motor_1_vel);
-  roboteqs_mock_->GetDriver()->SetVelocity(DriverChannel::CHANNEL2, motor_2_vel);
+  roboteq_mock_->GetDriver()->SetVelocity(DriverChannel::CHANNEL1, motor_1_vel);
+  roboteq_mock_->GetDriver()->SetVelocity(DriverChannel::CHANNEL2, motor_2_vel);
 
-  roboteqs_mock_->GetDriver()->SetCurrent(DriverChannel::CHANNEL1, motor_1_current);
-  roboteqs_mock_->GetDriver()->SetCurrent(DriverChannel::CHANNEL2, motor_2_current);
+  roboteq_mock_->GetDriver()->SetCurrent(DriverChannel::CHANNEL1, motor_1_current);
+  roboteq_mock_->GetDriver()->SetCurrent(DriverChannel::CHANNEL2, motor_2_current);
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
   panther_hardware_interfaces::MotorDriverState fb_motor_1 =
     roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadMotorDriverState();
@@ -174,7 +171,7 @@ TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps)
   panther_hardware_interfaces::MotorDriverState fb_motor_1 =
     roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadMotorDriverState();
 
-  // based on publishing frequency in the Roboteq mock (10)
+  // based on publishing frequency in the Roboteq mock (100Hz)
   std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
   panther_hardware_interfaces::MotorDriverState fb_motor_2 =
@@ -216,17 +213,17 @@ TEST_F(TestRoboteqDriver, ReadDriverState)
   const std::int16_t battery_current_1 = 10;
   const std::int16_t battery_current_2 = 30;
 
-  roboteqs_mock_->GetDriver()->SetTemperature(temp);
-  roboteqs_mock_->GetDriver()->SetHeatsinkTemperature(heatsink_temp);
-  roboteqs_mock_->GetDriver()->SetVoltage(volt);
-  roboteqs_mock_->GetDriver()->SetBatteryCurrent1(battery_current_1);
-  roboteqs_mock_->GetDriver()->SetBatteryCurrent2(battery_current_2);
+  roboteq_mock_->GetDriver()->SetTemperature(temp);
+  roboteq_mock_->GetDriver()->SetHeatsinkTemperature(heatsink_temp);
+  roboteq_mock_->GetDriver()->SetVoltage(volt);
+  roboteq_mock_->GetDriver()->SetBatteryCurrent1(battery_current_1);
+  roboteq_mock_->GetDriver()->SetBatteryCurrent2(battery_current_2);
 
-  roboteqs_mock_->GetDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERHEAT);
-  roboteqs_mock_->GetDriver()->SetDriverScriptFlag(DriverScriptFlags::ENCODER_DISCONNECTED);
-  roboteqs_mock_->GetDriver()->SetDriverRuntimeError(
+  roboteq_mock_->GetDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERHEAT);
+  roboteq_mock_->GetDriver()->SetDriverScriptFlag(DriverScriptFlags::ENCODER_DISCONNECTED);
+  roboteq_mock_->GetDriver()->SetDriverRuntimeError(
     DriverChannel::CHANNEL1, DriverRuntimeErrors::LOOP_ERROR);
-  roboteqs_mock_->GetDriver()->SetDriverRuntimeError(
+  roboteq_mock_->GetDriver()->SetDriverRuntimeError(
     DriverChannel::CHANNEL2, DriverRuntimeErrors::SAFETY_STOP_ACTIVE);
 
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
@@ -251,7 +248,7 @@ TEST_F(TestRoboteqDriver, ReadDriverStateTimestamp)
 
   panther_hardware_interfaces::DriverState fb1 = roboteq_driver_->ReadDriverState();
 
-  // based on publishing frequency in the Roboteq mock (50)
+  // based on publishing frequency in the Roboteq mock (20Hz)
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
   panther_hardware_interfaces::DriverState fb2 = roboteq_driver_->ReadDriverState();
@@ -293,44 +290,44 @@ TEST_F(TestRoboteqDriver, SendRoboteqCmd)
 
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), motor_1_v);
-  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), motor_2_v);
+  EXPECT_EQ(roboteq_mock_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), motor_1_v);
+  EXPECT_EQ(roboteq_mock_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), motor_2_v);
 }
 
 TEST_F(TestRoboteqDriver, ResetRoboteqScript)
 {
   BootRoboteqDriver();
-  roboteqs_mock_->GetDriver()->SetResetRoboteqScript(65);
+  roboteq_mock_->GetDriver()->SetResetRoboteqScript(65);
   roboteq_driver_->ResetScript();
 
-  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetResetRoboteqScript(), 2);
+  EXPECT_EQ(roboteq_mock_->GetDriver()->GetResetRoboteqScript(), 2);
 }
 
 TEST_F(TestRoboteqDriver, ResetRoboteqScriptSDOTimeoutReset)
 {
   BootRoboteqDriver();
 
-  roboteqs_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 100000);
+  roboteq_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 100000);
   EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { roboteq_driver_->ResetScript(); }, "SDO protocol timed out"));
 
-  roboteqs_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 0);
-  ASSERT_NO_THROW(roboteq_driver_->ResetScript());
+  roboteq_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 0);
+  EXPECT_NO_THROW(roboteq_driver_->ResetScript());
 }
 
-TEST_F(TestRoboteqDriver, ReadRoboteqTurnOnEStop)
+TEST_F(TestRoboteqDriver, RoboteqTurnOnEStop)
 {
   BootRoboteqDriver();
-  roboteqs_mock_->GetDriver()->SetTurnOnEStop(65);
+  roboteq_mock_->GetDriver()->SetTurnOnEStop(65);
   roboteq_driver_->TurnOnEStop();
 
-  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetTurnOnEStop(), 1);
+  EXPECT_EQ(roboteq_mock_->GetDriver()->GetTurnOnEStop(), 1);
 }
 
 TEST_F(TestRoboteqDriver, TurnOnEStopTimeout)
 {
   BootRoboteqDriver();
-  roboteqs_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200C, 0, 100000);
+  roboteq_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200C, 0, 100000);
   EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { roboteq_driver_->TurnOnEStop(); }, "SDO protocol timed out"));
 }
@@ -338,16 +335,16 @@ TEST_F(TestRoboteqDriver, TurnOnEStopTimeout)
 TEST_F(TestRoboteqDriver, TurnOffEStop)
 {
   BootRoboteqDriver();
-  roboteqs_mock_->GetDriver()->SetTurnOffEStop(65);
+  roboteq_mock_->GetDriver()->SetTurnOffEStop(65);
   roboteq_driver_->TurnOffEStop();
 
-  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetTurnOffEStop(), 1);
+  EXPECT_EQ(roboteq_mock_->GetDriver()->GetTurnOffEStop(), 1);
 }
 
 TEST_F(TestRoboteqDriver, TurnOffEStopTimeout)
 {
   BootRoboteqDriver();
-  roboteqs_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200D, 0, 100000);
+  roboteq_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200D, 0, 100000);
   EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { roboteq_driver_->TurnOffEStop(); }, "SDO protocol timed out"));
 }
@@ -355,25 +352,25 @@ TEST_F(TestRoboteqDriver, TurnOffEStopTimeout)
 TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel1)
 {
   BootRoboteqDriver();
-  roboteqs_mock_->GetDriver()->SetTurnOnSafetyStop(67);
+  roboteq_mock_->GetDriver()->SetTurnOnSafetyStop(67);
   roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop();
 
-  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetTurnOnSafetyStop(), 1);
+  EXPECT_EQ(roboteq_mock_->GetDriver()->GetTurnOnSafetyStop(), 1);
 }
 
 TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel2)
 {
   BootRoboteqDriver();
-  roboteqs_mock_->GetDriver()->SetTurnOnSafetyStop(65);
+  roboteq_mock_->GetDriver()->SetTurnOnSafetyStop(65);
   roboteq_driver_->GetMotorDriver(kMotor2Name)->TurnOnSafetyStop();
 
-  EXPECT_EQ(roboteqs_mock_->GetDriver()->GetTurnOnSafetyStop(), 2);
+  EXPECT_EQ(roboteq_mock_->GetDriver()->GetTurnOnSafetyStop(), 2);
 }
 
 TEST_F(TestRoboteqDriver, WriteTimeout)
 {
   BootRoboteqDriver();
-  roboteqs_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 200000);
+  roboteq_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 200000);
   EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop(); },
     "SDO protocol timed out"));
@@ -385,5 +382,7 @@ TEST_F(TestRoboteqDriver, WriteTimeout)
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
+
+  auto result = RUN_ALL_TESTS();
+  return result;
 }
diff --git a/panther_hardware_interfaces/test/utils/roboteqs_mock.hpp b/panther_hardware_interfaces/test/utils/roboteq_mock.hpp
similarity index 97%
rename from panther_hardware_interfaces/test/utils/roboteqs_mock.hpp
rename to panther_hardware_interfaces/test/utils/roboteq_mock.hpp
index 98edd777..378d4383 100644
--- a/panther_hardware_interfaces/test/utils/roboteqs_mock.hpp
+++ b/panther_hardware_interfaces/test/utils/roboteq_mock.hpp
@@ -258,13 +258,13 @@ class RoboteqSlave : public lely::canopen::BasicSlave
 };
 
 /**
- * @brief Class that simulates two Roboteqs
+ * @brief Class that simulates Roboteq controller
  */
-class RoboteqsMock
+class RoboteqMock
 {
 public:
-  RoboteqsMock() {}
-  ~RoboteqsMock() {}
+  RoboteqMock() {}
+  ~RoboteqMock() {}
 
   /**
    * @brief Starts CAN communication and creates a simulated Roboteq, that publish PDOs with set
@@ -291,11 +291,6 @@ class RoboteqsMock
             ament_index_cpp::get_package_share_directory("panther_hardware_interfaces")) /
           "test" / "config" / "slave_1.bin";
 
-        std::string slave2_eds_bin_path =
-          std::filesystem::path(
-            ament_index_cpp::get_package_share_directory("panther_hardware_interfaces")) /
-          "test" / "config" / "slave_2.bin";
-
         lely::io::IoGuard io_guard;
         lely::io::Poll poll(*ctx_);
         lely::ev::Loop loop(poll.get_poll());
@@ -336,7 +331,7 @@ class RoboteqsMock
   }
 
   /**
-   * @brief Stops CAN communication and removes simulated Roboteqs
+   * @brief Stops CAN communication and removes simulated Roboteq
    */
   void Stop()
   {

From 4140d115c95c65a31325e5fe4694b86c5d97ee4a Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Tue, 27 Aug 2024 13:40:30 +0000
Subject: [PATCH 025/100] mock first

---
 .../motors_controller/test_roboteq_driver.cpp | 80 +++++++++----------
 .../{roboteq_mock.hpp => mock_roboteq.hpp}    | 12 +--
 2 files changed, 46 insertions(+), 46 deletions(-)
 rename panther_hardware_interfaces/test/utils/{roboteq_mock.hpp => mock_roboteq.hpp} (97%)

diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
index 5edd9f97..4c002cab 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
@@ -24,7 +24,7 @@
 #include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp"
 
 #include "utils/fake_can_socket.hpp"
-#include "utils/roboteq_mock.hpp"
+#include "utils/mock_roboteq.hpp"
 #include "utils/test_constants.hpp"
 
 #include "panther_utils/test/test_utils.hpp"
@@ -45,7 +45,7 @@ class TestRoboteqDriver : public ::testing::Test
   static constexpr char kMotor2Name[] = "motor_2";
 
   std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
-  std::unique_ptr<panther_hardware_interfaces_test::RoboteqMock> roboteq_mock_;
+  std::unique_ptr<panther_hardware_interfaces_test::MockRoboteq> mock_roboteq_;
   std::unique_ptr<panther_hardware_interfaces::CANopenManager> canopen_manager_;
   std::shared_ptr<panther_hardware_interfaces::RoboteqDriver> roboteq_driver_;
 };
@@ -59,8 +59,8 @@ TestRoboteqDriver::TestRoboteqDriver()
   canopen_manager_ = std::make_unique<panther_hardware_interfaces::CANopenManager>(
     panther_hardware_interfaces_test::kCANopenSettings);
 
-  roboteq_mock_ = std::make_unique<panther_hardware_interfaces_test::RoboteqMock>();
-  roboteq_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
+  mock_roboteq_ = std::make_unique<panther_hardware_interfaces_test::MockRoboteq>();
+  mock_roboteq_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
   canopen_manager_->Initialize();
 
   roboteq_driver_ = std::make_shared<panther_hardware_interfaces::RoboteqDriver>(
@@ -71,8 +71,8 @@ TestRoboteqDriver::~TestRoboteqDriver()
 {
   roboteq_driver_.reset();
   canopen_manager_->Deinitialize();
-  roboteq_mock_->Stop();
-  roboteq_mock_.reset();
+  mock_roboteq_->Stop();
+  mock_roboteq_.reset();
   can_socket_->Deinitialize();
 }
 
@@ -113,7 +113,7 @@ TEST_F(TestRoboteqDriver, BootErrorDeviceType)
 {
   const auto device_type_id = 0x1000;
   const auto device_type_subid = 0;
-  roboteq_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(
+  mock_roboteq_->GetDriver()->SetOnReadWait<std::uint32_t>(
     device_type_id, device_type_subid, 100000);
   EXPECT_FALSE(TestBoot());
 }
@@ -122,7 +122,7 @@ TEST_F(TestRoboteqDriver, BootErrorVendorId)
 {
   const auto vendor_id_id = 0x1018;
   const auto vendor_id_subid = 1;
-  roboteq_mock_->GetDriver()->SetOnReadWait<std::uint32_t>(vendor_id_id, vendor_id_subid, 100000);
+  mock_roboteq_->GetDriver()->SetOnReadWait<std::uint32_t>(vendor_id_id, vendor_id_subid, 100000);
   EXPECT_FALSE(TestBoot());
 }
 
@@ -139,14 +139,14 @@ TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
 
   BootRoboteqDriver();
 
-  roboteq_mock_->GetDriver()->SetPosition(DriverChannel::CHANNEL1, motor_1_pos);
-  roboteq_mock_->GetDriver()->SetPosition(DriverChannel::CHANNEL2, motor_2_pos);
+  mock_roboteq_->GetDriver()->SetPosition(DriverChannel::CHANNEL1, motor_1_pos);
+  mock_roboteq_->GetDriver()->SetPosition(DriverChannel::CHANNEL2, motor_2_pos);
 
-  roboteq_mock_->GetDriver()->SetVelocity(DriverChannel::CHANNEL1, motor_1_vel);
-  roboteq_mock_->GetDriver()->SetVelocity(DriverChannel::CHANNEL2, motor_2_vel);
+  mock_roboteq_->GetDriver()->SetVelocity(DriverChannel::CHANNEL1, motor_1_vel);
+  mock_roboteq_->GetDriver()->SetVelocity(DriverChannel::CHANNEL2, motor_2_vel);
 
-  roboteq_mock_->GetDriver()->SetCurrent(DriverChannel::CHANNEL1, motor_1_current);
-  roboteq_mock_->GetDriver()->SetCurrent(DriverChannel::CHANNEL2, motor_2_current);
+  mock_roboteq_->GetDriver()->SetCurrent(DriverChannel::CHANNEL1, motor_1_current);
+  mock_roboteq_->GetDriver()->SetCurrent(DriverChannel::CHANNEL2, motor_2_current);
 
   std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
@@ -213,17 +213,17 @@ TEST_F(TestRoboteqDriver, ReadDriverState)
   const std::int16_t battery_current_1 = 10;
   const std::int16_t battery_current_2 = 30;
 
-  roboteq_mock_->GetDriver()->SetTemperature(temp);
-  roboteq_mock_->GetDriver()->SetHeatsinkTemperature(heatsink_temp);
-  roboteq_mock_->GetDriver()->SetVoltage(volt);
-  roboteq_mock_->GetDriver()->SetBatteryCurrent1(battery_current_1);
-  roboteq_mock_->GetDriver()->SetBatteryCurrent2(battery_current_2);
+  mock_roboteq_->GetDriver()->SetTemperature(temp);
+  mock_roboteq_->GetDriver()->SetHeatsinkTemperature(heatsink_temp);
+  mock_roboteq_->GetDriver()->SetVoltage(volt);
+  mock_roboteq_->GetDriver()->SetBatteryCurrent1(battery_current_1);
+  mock_roboteq_->GetDriver()->SetBatteryCurrent2(battery_current_2);
 
-  roboteq_mock_->GetDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERHEAT);
-  roboteq_mock_->GetDriver()->SetDriverScriptFlag(DriverScriptFlags::ENCODER_DISCONNECTED);
-  roboteq_mock_->GetDriver()->SetDriverRuntimeError(
+  mock_roboteq_->GetDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERHEAT);
+  mock_roboteq_->GetDriver()->SetDriverScriptFlag(DriverScriptFlags::ENCODER_DISCONNECTED);
+  mock_roboteq_->GetDriver()->SetDriverRuntimeError(
     DriverChannel::CHANNEL1, DriverRuntimeErrors::LOOP_ERROR);
-  roboteq_mock_->GetDriver()->SetDriverRuntimeError(
+  mock_roboteq_->GetDriver()->SetDriverRuntimeError(
     DriverChannel::CHANNEL2, DriverRuntimeErrors::SAFETY_STOP_ACTIVE);
 
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
@@ -290,44 +290,44 @@ TEST_F(TestRoboteqDriver, SendRoboteqCmd)
 
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  EXPECT_EQ(roboteq_mock_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), motor_1_v);
-  EXPECT_EQ(roboteq_mock_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), motor_2_v);
+  EXPECT_EQ(mock_roboteq_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), motor_1_v);
+  EXPECT_EQ(mock_roboteq_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), motor_2_v);
 }
 
 TEST_F(TestRoboteqDriver, ResetRoboteqScript)
 {
   BootRoboteqDriver();
-  roboteq_mock_->GetDriver()->SetResetRoboteqScript(65);
+  mock_roboteq_->GetDriver()->SetResetRoboteqScript(65);
   roboteq_driver_->ResetScript();
 
-  EXPECT_EQ(roboteq_mock_->GetDriver()->GetResetRoboteqScript(), 2);
+  EXPECT_EQ(mock_roboteq_->GetDriver()->GetResetRoboteqScript(), 2);
 }
 
 TEST_F(TestRoboteqDriver, ResetRoboteqScriptSDOTimeoutReset)
 {
   BootRoboteqDriver();
 
-  roboteq_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 100000);
+  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 100000);
   EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { roboteq_driver_->ResetScript(); }, "SDO protocol timed out"));
 
-  roboteq_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 0);
+  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 0);
   EXPECT_NO_THROW(roboteq_driver_->ResetScript());
 }
 
 TEST_F(TestRoboteqDriver, RoboteqTurnOnEStop)
 {
   BootRoboteqDriver();
-  roboteq_mock_->GetDriver()->SetTurnOnEStop(65);
+  mock_roboteq_->GetDriver()->SetTurnOnEStop(65);
   roboteq_driver_->TurnOnEStop();
 
-  EXPECT_EQ(roboteq_mock_->GetDriver()->GetTurnOnEStop(), 1);
+  EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOnEStop(), 1);
 }
 
 TEST_F(TestRoboteqDriver, TurnOnEStopTimeout)
 {
   BootRoboteqDriver();
-  roboteq_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200C, 0, 100000);
+  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200C, 0, 100000);
   EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { roboteq_driver_->TurnOnEStop(); }, "SDO protocol timed out"));
 }
@@ -335,16 +335,16 @@ TEST_F(TestRoboteqDriver, TurnOnEStopTimeout)
 TEST_F(TestRoboteqDriver, TurnOffEStop)
 {
   BootRoboteqDriver();
-  roboteq_mock_->GetDriver()->SetTurnOffEStop(65);
+  mock_roboteq_->GetDriver()->SetTurnOffEStop(65);
   roboteq_driver_->TurnOffEStop();
 
-  EXPECT_EQ(roboteq_mock_->GetDriver()->GetTurnOffEStop(), 1);
+  EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOffEStop(), 1);
 }
 
 TEST_F(TestRoboteqDriver, TurnOffEStopTimeout)
 {
   BootRoboteqDriver();
-  roboteq_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200D, 0, 100000);
+  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200D, 0, 100000);
   EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { roboteq_driver_->TurnOffEStop(); }, "SDO protocol timed out"));
 }
@@ -352,25 +352,25 @@ TEST_F(TestRoboteqDriver, TurnOffEStopTimeout)
 TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel1)
 {
   BootRoboteqDriver();
-  roboteq_mock_->GetDriver()->SetTurnOnSafetyStop(67);
+  mock_roboteq_->GetDriver()->SetTurnOnSafetyStop(67);
   roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop();
 
-  EXPECT_EQ(roboteq_mock_->GetDriver()->GetTurnOnSafetyStop(), 1);
+  EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOnSafetyStop(), 1);
 }
 
 TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel2)
 {
   BootRoboteqDriver();
-  roboteq_mock_->GetDriver()->SetTurnOnSafetyStop(65);
+  mock_roboteq_->GetDriver()->SetTurnOnSafetyStop(65);
   roboteq_driver_->GetMotorDriver(kMotor2Name)->TurnOnSafetyStop();
 
-  EXPECT_EQ(roboteq_mock_->GetDriver()->GetTurnOnSafetyStop(), 2);
+  EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOnSafetyStop(), 2);
 }
 
 TEST_F(TestRoboteqDriver, WriteTimeout)
 {
   BootRoboteqDriver();
-  roboteq_mock_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 200000);
+  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 200000);
   EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop(); },
     "SDO protocol timed out"));
diff --git a/panther_hardware_interfaces/test/utils/roboteq_mock.hpp b/panther_hardware_interfaces/test/utils/mock_roboteq.hpp
similarity index 97%
rename from panther_hardware_interfaces/test/utils/roboteq_mock.hpp
rename to panther_hardware_interfaces/test/utils/mock_roboteq.hpp
index 378d4383..c863daf4 100644
--- a/panther_hardware_interfaces/test/utils/roboteq_mock.hpp
+++ b/panther_hardware_interfaces/test/utils/mock_roboteq.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_TEST_UTILS_ROBOTEQ_MOCK_HPP_
-#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_ROBOTEQ_MOCK_HPP_
+#ifndef PANTHER_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_
+#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_
 
 #include <atomic>
 #include <condition_variable>
@@ -260,11 +260,11 @@ class RoboteqSlave : public lely::canopen::BasicSlave
 /**
  * @brief Class that simulates Roboteq controller
  */
-class RoboteqMock
+class MockRoboteq
 {
 public:
-  RoboteqMock() {}
-  ~RoboteqMock() {}
+  MockRoboteq() {}
+  ~MockRoboteq() {}
 
   /**
    * @brief Starts CAN communication and creates a simulated Roboteq, that publish PDOs with set
@@ -361,4 +361,4 @@ class RoboteqMock
 
 }  // namespace panther_hardware_interfaces_test
 
-#endif  // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_ROBOTEQ_MOCK_HPP_
+#endif  // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_

From da2da07d1bf3ba1c1dd67a4e8b0a2d3e3009bb58 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Tue, 27 Aug 2024 13:45:13 +0000
Subject: [PATCH 026/100] fix merge

---
 .../src/panther_system/robot_driver/roboteq_driver.cpp          | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp
index 969424f3..191f9162 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp
@@ -24,7 +24,7 @@
 #include <string>
 #include <system_error>
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
 #include "panther_hardware_interfaces/utils.hpp"
 
 namespace panther_hardware_interfaces

From f142baa228b6054c1988916e961f94a0bfe53e1c Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Tue, 27 Aug 2024 13:49:04 +0000
Subject: [PATCH 027/100] add lynx robot driver

---
 panther_hardware_interfaces/CMakeLists.txt    | 145 ++++---
 .../robot_driver/lynx_robot_driver.hpp        | 182 ++++++++
 .../robot_driver/lynx_robot_driver.cpp        | 228 ++++++++++
 .../robot_driver/test_lynx_robot_driver.cpp   | 409 ++++++++++++++++++
 4 files changed, 894 insertions(+), 70 deletions(-)
 create mode 100644 panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
 create mode 100644 panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
 create mode 100644 panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp

diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt
index a8cd31b0..5b646f67 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/panther_hardware_interfaces/CMakeLists.txt
@@ -51,18 +51,19 @@ pkg_check_modules(LIBLELY_COAPP REQUIRED IMPORTED_TARGET liblely-coapp)
 pkg_check_modules(LIBGPIOD REQUIRED IMPORTED_TARGET libgpiodcxx)
 
 add_library(
-  ${PROJECT_NAME} SHARED
-  src/panther_imu_sensor/panther_imu_sensor.cpp
-  src/panther_system/gpio/gpio_controller.cpp
-  src/panther_system/gpio/gpio_driver.cpp
-  src/panther_system/robot_driver/canopen_manager.cpp
-  src/panther_system/robot_driver/robot_driver.cpp
-  src/panther_system/robot_driver/roboteq_data_converters.cpp
-  src/panther_system/robot_driver/roboteq_driver.cpp
-  src/panther_system/robot_driver/roboteq_error_filter.cpp
-  src/panther_system/panther_system_e_stop.cpp
-  src/panther_system/panther_system_ros_interface.cpp
-  src/panther_system/panther_system.cpp
+  ${PROJECT_NAME}
+  SHARED
+  # src/panther_imu_sensor/panther_imu_sensor.cpp
+  # src/panther_system/gpio/gpio_controller.cpp
+  # src/panther_system/gpio/gpio_driver.cpp
+  # src/panther_system/robot_driver/canopen_manager.cpp
+  # src/panther_system/robot_driver/robot_driver.cpp
+  # src/panther_system/robot_driver/roboteq_data_converters.cpp
+  # src/panther_system/robot_driver/roboteq_driver.cpp
+  # src/panther_system/robot_driver/roboteq_error_filter.cpp
+  # src/panther_system/panther_system_e_stop.cpp
+  # src/panther_system/panther_system_ros_interface.cpp
+  # src/panther_system/panther_system.cpp
   src/utils.cpp)
 ament_target_dependencies(${PROJECT_NAME} ${PACKAGE_DEPENDENCIES})
 target_link_libraries(${PROJECT_NAME} PkgConfig::LIBLELY_COAPP
@@ -89,15 +90,15 @@ if(BUILD_TESTING)
 
   install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)
 
-  ament_add_gtest(${PROJECT_NAME}_test_utils test/test_utils.cpp src/utils.cpp)
+  # ament_add_gtest(${PROJECT_NAME}_test_utils test/test_utils.cpp
+  # src/utils.cpp)
 
-  ament_add_gtest(${PROJECT_NAME}_test_panther_imu_sensor
-                  test/panther_imu_sensor/test_panther_imu_sensor.cpp)
-  ament_target_dependencies(
-    ${PROJECT_NAME}_test_panther_imu_sensor hardware_interface rclcpp
-    panther_utils panther_msgs phidgets_api)
-  target_link_libraries(${PROJECT_NAME}_test_panther_imu_sensor ${PROJECT_NAME}
-                        phidgets_spatial_parameters)
+  # ament_add_gtest(${PROJECT_NAME}_test_panther_imu_sensor
+  # test/panther_imu_sensor/test_panther_imu_sensor.cpp)
+  # ament_target_dependencies( ${PROJECT_NAME}_test_panther_imu_sensor
+  # hardware_interface rclcpp panther_utils panther_msgs phidgets_api)
+  # target_link_libraries(${PROJECT_NAME}_test_panther_imu_sensor
+  # ${PROJECT_NAME} phidgets_spatial_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_error_filter
@@ -146,7 +147,7 @@ if(BUILD_TESTING)
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gmock(
-    ${PROJECT_NAME}_test_robot_driver
+    ${PROJECT_NAME}_test_panther_robot_driver
     test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
     src/panther_system/robot_driver/canopen_manager.cpp
     src/panther_system/robot_driver/roboteq_driver.cpp
@@ -154,66 +155,70 @@ if(BUILD_TESTING)
     src/panther_system/robot_driver/robot_driver.cpp
     src/utils.cpp)
   target_include_directories(
-    ${PROJECT_NAME}_test_robot_driver
+    ${PROJECT_NAME}_test_panther_robot_driver
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
-  ament_target_dependencies(${PROJECT_NAME}_test_robot_driver rclcpp
+  ament_target_dependencies(${PROJECT_NAME}_test_panther_robot_driver rclcpp
                             panther_msgs panther_utils)
-  target_link_libraries(${PROJECT_NAME}_test_robot_driver
+  target_link_libraries(${PROJECT_NAME}_test_panther_robot_driver
                         PkgConfig::LIBLELY_COAPP)
 
-  ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver
-                  test/panther_system/gpio/test_gpio_driver.cpp)
-  ament_target_dependencies(${PROJECT_NAME}_test_gpiod_driver panther_utils)
-  target_link_libraries(${PROJECT_NAME}_test_gpiod_driver ${PROJECT_NAME}
-                        PkgConfig::LIBGPIOD)
-
-  ament_add_gtest(
-    ${PROJECT_NAME}_test_gpiod_controller
-    test/panther_system/gpio/test_gpio_controller.cpp
-    src/panther_system/gpio/gpio_controller.cpp
-    src/panther_system/gpio/gpio_driver.cpp)
-  ament_target_dependencies(${PROJECT_NAME}_test_gpiod_controller panther_utils)
-  target_link_libraries(${PROJECT_NAME}_test_gpiod_controller
-                        PkgConfig::LIBGPIOD)
-
-  ament_add_gtest(
-    ${PROJECT_NAME}_test_panther_system_ros_interface
-    test/panther_system/test_panther_system_ros_interface.cpp
-    src/panther_system/panther_system_ros_interface.cpp
+  ament_add_gmock(
+    ${PROJECT_NAME}_test_lynx_robot_driver
+    test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
+    src/panther_system/robot_driver/canopen_manager.cpp
+    src/panther_system/robot_driver/roboteq_driver.cpp
     src/panther_system/robot_driver/roboteq_data_converters.cpp
-    src/utils.cpp
-    src/panther_system/gpio/gpio_controller.cpp
-    src/panther_system/gpio/gpio_driver.cpp)
-  target_include_directories(
-    ${PROJECT_NAME}_test_panther_system_ros_interface
-    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
-           $<INSTALL_INTERFACE:include>)
-  ament_target_dependencies(
-    ${PROJECT_NAME}_test_panther_system_ros_interface
-    diagnostic_updater
-    rclcpp
-    panther_msgs
-    panther_utils
-    realtime_tools
-    std_srvs)
-  target_link_libraries(${PROJECT_NAME}_test_panther_system_ros_interface
-                        PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD)
-
-  ament_add_gtest(${PROJECT_NAME}_test_panther_system
-                  test/panther_system/test_panther_system.cpp)
-  set_tests_properties(${PROJECT_NAME}_test_panther_system PROPERTIES TIMEOUT
-                                                                      120)
+    src/panther_system/robot_driver/robot_driver.cpp
+    src/panther_system/robot_driver/lynx_robot_driver.cpp
+    src/utils.cpp)
   target_include_directories(
-    ${PROJECT_NAME}_test_panther_system
+    ${PROJECT_NAME}_test_lynx_robot_driver
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
-  ament_target_dependencies(
-    ${PROJECT_NAME}_test_panther_system hardware_interface rclcpp panther_msgs
-    panther_utils)
-  target_link_libraries(${PROJECT_NAME}_test_panther_system
+  ament_target_dependencies(${PROJECT_NAME}_test_lynx_robot_driver rclcpp
+                            panther_msgs panther_utils)
+  target_link_libraries(${PROJECT_NAME}_test_lynx_robot_driver
                         PkgConfig::LIBLELY_COAPP)
 
+  # ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver
+  # test/panther_system/gpio/test_gpio_driver.cpp)
+  # ament_target_dependencies(${PROJECT_NAME}_test_gpiod_driver panther_utils)
+  # target_link_libraries(${PROJECT_NAME}_test_gpiod_driver ${PROJECT_NAME}
+  # PkgConfig::LIBGPIOD)
+
+  # ament_add_gtest( ${PROJECT_NAME}_test_gpiod_controller
+  # test/panther_system/gpio/test_gpio_controller.cpp
+  # src/panther_system/gpio/gpio_controller.cpp
+  # src/panther_system/gpio/gpio_driver.cpp)
+  # ament_target_dependencies(${PROJECT_NAME}_test_gpiod_controller
+  # panther_utils) target_link_libraries(${PROJECT_NAME}_test_gpiod_controller
+  # PkgConfig::LIBGPIOD)
+
+  # ament_add_gtest( ${PROJECT_NAME}_test_panther_system_ros_interface
+  # test/panther_system/test_panther_system_ros_interface.cpp
+  # src/panther_system/panther_system_ros_interface.cpp
+  # src/panther_system/robot_driver/roboteq_data_converters.cpp src/utils.cpp
+  # src/panther_system/gpio/gpio_controller.cpp
+  # src/panther_system/gpio/gpio_driver.cpp) target_include_directories(
+  # ${PROJECT_NAME}_test_panther_system_ros_interface PUBLIC
+  # $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
+  # $<INSTALL_INTERFACE:include>) ament_target_dependencies(
+  # ${PROJECT_NAME}_test_panther_system_ros_interface diagnostic_updater rclcpp
+  # panther_msgs panther_utils realtime_tools std_srvs)
+  # target_link_libraries(${PROJECT_NAME}_test_panther_system_ros_interface
+  # PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD)
+
+  # ament_add_gtest(${PROJECT_NAME}_test_panther_system
+  # test/panther_system/test_panther_system.cpp)
+  # set_tests_properties(${PROJECT_NAME}_test_panther_system PROPERTIES TIMEOUT
+  # 120) target_include_directories( ${PROJECT_NAME}_test_panther_system PUBLIC
+  # $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
+  # $<INSTALL_INTERFACE:include>) ament_target_dependencies(
+  # ${PROJECT_NAME}_test_panther_system hardware_interface rclcpp panther_msgs
+  # panther_utils) target_link_libraries(${PROJECT_NAME}_test_panther_system
+  # PkgConfig::LIBLELY_COAPP)
+
 endif()
 
 ament_export_include_directories(include)
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
new file mode 100644
index 00000000..359cb91e
--- /dev/null
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
@@ -0,0 +1,182 @@
+// 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_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_LYNX_ROBOT_DRIVER_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_LYNX_ROBOT_DRIVER_HPP_
+
+#include <chrono>
+
+#include <panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp>
+#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
+
+namespace panther_hardware_interfaces
+{
+
+struct LynxMotorNames
+{
+  static constexpr char LEFT[] = "left";
+  static constexpr char RIGHT[] = "right";
+};
+
+struct LynxDriverNames
+{
+  static constexpr char DEFAULT[] = "default";
+};
+
+struct LynxMotorChannel
+{
+  static constexpr std::uint8_t LEFT = RoboteqDriver::kChannel2;
+  static constexpr std::uint8_t RIGHT = RoboteqDriver::kChannel1;
+};
+
+/**
+ * @brief This class abstracts the usage of one Roboteq controller.
+ * It uses canopen_manager for communication with Roboteq controller,
+ * implements the activation procedure for controller (resets script and sends initial 0 command),
+ * and provides methods to get data feedback and send commands.
+ * Data is converted between raw Roboteq formats and SI units using roboteq_data_converters.
+ */
+class LynxRobotDriver : public RobotDriver
+{
+public:
+  LynxRobotDriver(
+    const std::shared_ptr<Driver> driver, const CANopenSettings & canopen_settings,
+    const DrivetrainSettings & drivetrain_settings,
+    const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000));
+
+  ~LynxRobotDriver()
+  {
+    driver_.reset();
+    Deinitialize();
+  };
+
+  /**
+   * @brief Starts CAN communication and waits for boot to finish
+   *
+   * @exception std::runtime_error if boot fails
+   */
+  void Initialize() override;
+
+  /**
+   * @brief Deinitialize can communication
+   */
+  void Deinitialize() override;
+
+  /**
+   * @brief Activate procedure for Roboteq drivers - reset scripts and send 0 commands on both
+   * channels. Blocking function, takes around 2 seconds to finish
+   *
+   * @exception std::runtime_error if any procedure step fails
+   */
+  void Activate() override;
+
+  /**
+   * @brief Updates current communication state with Roboteq drivers
+   *
+   * @exception std::runtime_error if CAN error was detected
+   */
+  void UpdateCommunicationState() override;
+
+  /**
+   * @brief Updates current motors' state (position, velocity, current).
+   *
+   * @exception std::runtime_error if CAN error was detected
+   */
+  void UpdateMotorsState() override;
+
+  /**
+   * @brief Updates current Roboteq driver state (flags, temperatures, voltage, battery current)
+   *
+   * @exception std::runtime_error if CAN error was detected
+   */
+  void UpdateDriversState() override;
+
+  /**
+   * @brief Get data feedback from the driver
+   *
+   * @param name name of the data to get
+   *
+   * @return data feedback
+   * @exception std::runtime_error if data with the given name does not exist
+   */
+  const RoboteqData & GetData(const std::string & name) override;
+
+  /**
+   * @brief Write speed commands to motors
+   *
+   * @param speed_fl front left motor speed in rad/s
+   * @param speed_fr front right motor speed in rad/s
+   * @param speed_rl rear left motor speed in rad/s
+   * @param speed_rr rear right motor speed in rad/s
+   *
+   * @exception std::runtime_error if send command fails or CAN error was detected
+   */
+  void SendSpeedCommands(
+    const float speed_left, const float speed_right, const float /*speed_rl*/,
+    const float /*speed_rr*/) override;
+
+  /**
+   * @brief Turns on Roboteq E-stop
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  void TurnOnEStop() override;
+
+  /**
+   * @brief Turns off Roboteq E-stop
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  void TurnOffEStop() override;
+
+  /**
+   * @brief Turns on Roboteq safety stop. To turn it off, it is necessary to send
+   * 0 commands to motors.
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  void TurnOnSafetyStop() override;
+
+  /**
+   * @brief Attempt to clear driver error flags by sending 0 velocity commands to motors. If Roboteq
+   * driver faults still exist, the error flag will remain active.
+   */
+  inline void AttemptErrorFlagResetWithZeroSpeed() { SendSpeedCommands(0.0, 0.0, 0.0, 0.0); };
+
+private:
+  void SetMotorsStates(
+    RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
+    const timespec & current_time);
+  void SetDriverState(RoboteqData & data, const DriverState & state, const timespec & current_time);
+
+  bool initialized_ = false;
+
+  CANopenManager canopen_manager_;
+
+  std::shared_ptr<Driver> driver_;
+
+  RoboteqData data_;
+
+  RoboteqVelocityCommandConverter roboteq_vel_cmd_converter_;
+
+  const std::chrono::milliseconds pdo_motor_states_timeout_ms_;
+  const std::chrono::milliseconds pdo_driver_state_timeout_ms_;
+  const std::chrono::milliseconds activate_wait_time_;
+};
+
+}  // namespace panther_hardware_interfaces
+
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_LYNX_ROBOT_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
new file mode 100644
index 00000000..fba24edc
--- /dev/null
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
@@ -0,0 +1,228 @@
+// 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_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp"
+
+#include <chrono>
+#include <ctime>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <thread>
+#include <utility>
+
+#include "lely/util/chrono.hpp"
+
+#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
+
+namespace panther_hardware_interfaces
+{
+
+LynxRobotDriver::LynxRobotDriver(
+  const std::shared_ptr<Driver> driver, const CANopenSettings & canopen_settings,
+  const DrivetrainSettings & drivetrain_settings,
+  const std::chrono::milliseconds activate_wait_time)
+: canopen_manager_(canopen_settings),
+  driver_(std::move(driver)),
+  data_(drivetrain_settings),
+  roboteq_vel_cmd_converter_(drivetrain_settings),
+  pdo_motor_states_timeout_ms_(canopen_settings.pdo_motor_states_timeout_ms),
+  pdo_driver_state_timeout_ms_(canopen_settings.pdo_driver_state_timeout_ms),
+  activate_wait_time_(activate_wait_time)
+{
+}
+
+void LynxRobotDriver::Initialize()
+{
+  if (initialized_) {
+    return;
+  }
+
+  try {
+    canopen_manager_.Initialize();
+    driver_->Boot();
+  } catch (const std::runtime_error & e) {
+    throw e;
+  }
+
+  initialized_ = true;
+}
+
+void LynxRobotDriver::Deinitialize()
+{
+  canopen_manager_.Deinitialize();
+  initialized_ = false;
+}
+
+void LynxRobotDriver::Activate()
+{
+  // Activation procedure - it is necessary to first reset scripts, wait for a bit (1 second)
+  // and then send 0 commands for some time (also 1 second)
+
+  try {
+    driver_->ResetScript();
+  } catch (const std::runtime_error & e) {
+    throw std::runtime_error(
+      "Front driver reset Roboteq script exception: " + std::string(e.what()));
+  }
+
+  std::this_thread::sleep_for(activate_wait_time_);
+
+  try {
+    driver_->GetMotorDriver(LynxMotorNames::LEFT)->SendCmdVel(0);
+    driver_->GetMotorDriver(LynxMotorNames::RIGHT)->SendCmdVel(0);
+  } catch (const std::runtime_error & e) {
+    throw std::runtime_error("Front driver send 0 command exception: " + std::string(e.what()));
+  }
+
+  std::this_thread::sleep_for(activate_wait_time_);
+}
+
+void LynxRobotDriver::UpdateCommunicationState()
+{
+  data_.SetCANError(driver_->IsCANError());
+  data_.SetHeartbeatTimeout(driver_->IsHeartbeatTimeout());
+}
+
+void LynxRobotDriver::UpdateMotorsState()
+{
+  timespec current_time;
+  clock_gettime(CLOCK_MONOTONIC, &current_time);
+
+  const auto fl_state = driver_->GetMotorDriver(LynxMotorNames::LEFT)->ReadMotorDriverState();
+  const auto fr_state = driver_->GetMotorDriver(LynxMotorNames::RIGHT)->ReadMotorDriverState();
+
+  SetMotorsStates(data_, fl_state, fr_state, current_time);
+
+  UpdateCommunicationState();
+
+  if (data_.IsCANError()) {
+    throw std::runtime_error("CAN error.");
+  }
+
+  if (data_.IsHeartbeatTimeout()) {
+    throw std::runtime_error("Motor controller heartbeat timeout.");
+  }
+}
+
+void LynxRobotDriver::UpdateDriversState()
+{
+  timespec current_time;
+  clock_gettime(CLOCK_MONOTONIC, &current_time);
+
+  SetDriverState(data_, driver_->ReadDriverState(), current_time);
+
+  UpdateCommunicationState();
+
+  if (data_.IsCANError()) {
+    throw std::runtime_error("CAN error.");
+  }
+
+  if (data_.IsHeartbeatTimeout()) {
+    throw std::runtime_error("Motor controller heartbeat timeout.");
+  }
+}
+
+const RoboteqData & LynxRobotDriver::GetData(const std::string & name)
+{
+  if (name == LynxDriverNames::DEFAULT) {
+    return data_;
+  } else {
+    throw std::runtime_error("Data with name '" + name + "' does not exist.");
+  }
+}
+
+void LynxRobotDriver::SendSpeedCommands(
+  const float speed_left, const float speed_right, const float /*speed_rl*/,
+  const float /*speed_rr*/)
+{
+  // Channel 1 - right motor, Channel 2 - left motor
+  try {
+    driver_->GetMotorDriver(LynxMotorNames::LEFT)
+      ->SendCmdVel(roboteq_vel_cmd_converter_.Convert(speed_left));
+    driver_->GetMotorDriver(LynxMotorNames::RIGHT)
+      ->SendCmdVel(roboteq_vel_cmd_converter_.Convert(speed_right));
+  } catch (const std::runtime_error & e) {
+    throw std::runtime_error("Front driver send Roboteq cmd failed: " + std::string(e.what()));
+  }
+
+  if (driver_->IsCANError()) {
+    throw std::runtime_error(
+      "CAN error detected on the front driver when trying to write speed commands.");
+  }
+}
+
+void LynxRobotDriver::TurnOnEStop()
+{
+  try {
+    driver_->TurnOnEStop();
+  } catch (const std::runtime_error & e) {
+    throw std::runtime_error(
+      "Failed to turn on E-stop on the front driver: " + std::string(e.what()));
+  }
+}
+
+void LynxRobotDriver::TurnOffEStop()
+{
+  try {
+    driver_->TurnOffEStop();
+  } catch (const std::runtime_error & e) {
+    throw std::runtime_error(
+      "Failed to turn off E-stop on the front driver: " + std::string(e.what()));
+  }
+}
+
+void LynxRobotDriver::TurnOnSafetyStop()
+{
+  try {
+    driver_->GetMotorDriver(LynxMotorNames::LEFT)->TurnOnSafetyStop();
+    driver_->GetMotorDriver(LynxMotorNames::RIGHT)->TurnOnSafetyStop();
+  } catch (const std::runtime_error & e) {
+    throw std::runtime_error(
+      "Failed to turn on safety stop on the front driver: " + std::string(e.what()));
+  }
+}
+
+void LynxRobotDriver::SetMotorsStates(
+  RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
+  const timespec & current_time)
+{
+  // TODO figure out both motors timestamps
+  bool data_timed_out =
+    (lely::util::from_timespec(current_time) - lely::util::from_timespec(left_state.pos_timestamp) >
+     pdo_motor_states_timeout_ms_) ||
+    (lely::util::from_timespec(current_time) -
+       lely::util::from_timespec(left_state.vel_current_timestamp) >
+     pdo_motor_states_timeout_ms_);
+
+  // Channel 1 - right, Channel 2 - left
+  data.SetMotorsStates(right_state, left_state, data_timed_out);
+}
+
+void LynxRobotDriver::SetDriverState(
+  RoboteqData & data, const DriverState & state, const timespec & current_time)
+{
+  bool data_timed_out = (lely::util::from_timespec(current_time) -
+                           lely::util::from_timespec(state.flags_current_timestamp) >
+                         pdo_driver_state_timeout_ms_) ||
+                        (lely::util::from_timespec(current_time) -
+                           lely::util::from_timespec(state.voltages_temps_timestamp) >
+                         pdo_driver_state_timeout_ms_);
+
+  data.SetDriverState(state, data_timed_out);
+}
+
+}  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
new file mode 100644
index 00000000..ad5c01e6
--- /dev/null
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
@@ -0,0 +1,409 @@
+// Copyright 2023 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 <chrono>
+#include <cstdint>
+#include <future>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <thread>
+
+#include <gmock/gmock.h>
+
+#include <panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp>
+#include <panther_hardware_interfaces/panther_system/robot_driver/driver.hpp>
+#include <panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp>
+#include <panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp>
+
+#include "utils/fake_can_socket.hpp"
+#include "utils/test_constants.hpp"
+
+class MockDriver : public panther_hardware_interfaces::Driver
+{
+public:
+  MOCK_METHOD(std::future<void>, Boot, (), (override));
+  MOCK_METHOD(bool, IsCANError, (), (const, override));
+  MOCK_METHOD(bool, IsHeartbeatTimeout, (), (const, override));
+
+  MOCK_METHOD(panther_hardware_interfaces::DriverState, ReadDriverState, (), (override));
+  MOCK_METHOD(void, ResetScript, (), (override));
+  MOCK_METHOD(void, TurnOnEStop, (), (override));
+  MOCK_METHOD(void, TurnOffEStop, (), (override));
+
+  std::shared_ptr<panther_hardware_interfaces::MotorDriver> GetMotorDriver(
+    const std::string & name) override
+  {
+    return motor_drivers_.at(name);
+  }
+
+  void AddMotorDriver(
+    const std::string name,
+    std::shared_ptr<panther_hardware_interfaces::MotorDriver> motor_driver) override
+  {
+    motor_drivers_.emplace(name, motor_driver);
+  }
+
+private:
+  std::map<std::string, std::shared_ptr<panther_hardware_interfaces::MotorDriver>> motor_drivers_;
+};
+
+class MockMotorDriver : public panther_hardware_interfaces::MotorDriver
+{
+public:
+  MOCK_METHOD(panther_hardware_interfaces::MotorDriverState, ReadMotorDriverState, (), (override));
+  MOCK_METHOD(void, SendCmdVel, (const std::int32_t cmd), (override));
+  MOCK_METHOD(void, TurnOnSafetyStop, (), (override));
+};
+
+class TestLynxRobotDriverInitialization : public ::testing::Test
+{
+public:
+  TestLynxRobotDriverInitialization()
+  {
+    can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
+      panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
+    can_socket_->Initialize();
+
+    left_motor_driver_ = std::make_shared<::testing::NiceMock<MockMotorDriver>>();
+    right_motor_driver_ = std::make_shared<::testing::NiceMock<MockMotorDriver>>();
+
+    driver_mock_ = std::make_shared<::testing::NiceMock<MockDriver>>();
+    driver_mock_->AddMotorDriver(kLeftMotorDriverName, left_motor_driver_);
+    driver_mock_->AddMotorDriver(kRightMotorDriverName, right_motor_driver_);
+
+    robot_driver_ = std::make_unique<panther_hardware_interfaces::LynxRobotDriver>(
+      driver_mock_, panther_hardware_interfaces_test::kCANopenSettings,
+      panther_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10));
+  }
+
+  ~TestLynxRobotDriverInitialization() {}
+
+protected:
+  static constexpr char kDriverName[] = "default";
+  static constexpr char kLeftMotorDriverName[] = "left";
+  static constexpr char kRightMotorDriverName[] = "right";
+
+  std::shared_ptr<::testing::NiceMock<MockDriver>> driver_mock_;
+  std::shared_ptr<::testing::NiceMock<MockMotorDriver>> left_motor_driver_;
+  std::shared_ptr<::testing::NiceMock<MockMotorDriver>> right_motor_driver_;
+  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
+  std::unique_ptr<panther_hardware_interfaces::LynxRobotDriver> robot_driver_;
+};
+
+TEST_F(TestLynxRobotDriverInitialization, Initialize)
+{
+  EXPECT_CALL(*driver_mock_, Boot()).Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->Initialize());
+  ASSERT_NO_THROW(robot_driver_->Deinitialize());
+
+  EXPECT_CALL(*driver_mock_, Boot()).Times(1);
+  // Check if deinitialization worked correctly - initialize once again
+  ASSERT_NO_THROW(robot_driver_->Initialize());
+  ASSERT_NO_THROW(robot_driver_->Deinitialize());
+}
+
+TEST_F(TestLynxRobotDriverInitialization, Activate)
+{
+  EXPECT_CALL(*driver_mock_, ResetScript()).Times(1);
+  EXPECT_CALL(*left_motor_driver_, SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*right_motor_driver_, SendCmdVel(::testing::Eq(0))).Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->Initialize());
+  ASSERT_NO_THROW(robot_driver_->Activate());
+}
+
+class TestLynxRobotDriver : public TestLynxRobotDriverInitialization
+{
+public:
+  TestLynxRobotDriver()
+  {
+    robot_driver_->Initialize();
+    robot_driver_->Activate();
+  }
+
+  ~TestLynxRobotDriver() { robot_driver_->Deinitialize(); }
+};
+
+TEST_F(TestLynxRobotDriver, UpdateCommunicationState)
+{
+  EXPECT_CALL(*driver_mock_, IsCANError()).Times(1);
+  EXPECT_CALL(*driver_mock_, IsHeartbeatTimeout()).Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
+}
+
+TEST_F(TestLynxRobotDriver, UpdateCommunicationStateCANErorr)
+{
+  EXPECT_CALL(*driver_mock_, IsCANError()).WillOnce(::testing::Return(true));
+
+  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
+
+  EXPECT_TRUE(robot_driver_->GetData(kDriverName).IsCANError());
+}
+
+TEST_F(TestLynxRobotDriver, UpdateCommunicationStateHeartbeatTimeout)
+{
+  EXPECT_CALL(*driver_mock_, IsHeartbeatTimeout()).WillOnce(::testing::Return(true));
+
+  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
+
+  EXPECT_TRUE(robot_driver_->GetData(kDriverName).IsHeartbeatTimeout());
+}
+
+TEST_F(TestLynxRobotDriver, UpdateMotorsState)
+{
+  using panther_hardware_interfaces::LynxMotorChannel;
+  using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  using panther_hardware_interfaces_test::kRbtqPosFbToRad;
+  using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+
+  const std::int32_t l_pos = 101;
+  const std::int32_t l_vel = 102;
+  const std::int32_t l_current = 103;
+  const std::int32_t r_pos = 201;
+  const std::int32_t r_vel = 202;
+  const std::int32_t r_current = 203;
+
+  ON_CALL(*left_motor_driver_, ReadMotorDriverState())
+    .WillByDefault(::testing::Return(
+      panther_hardware_interfaces::MotorDriverState({l_pos, l_vel, l_current, {0, 0}, {0, 0}})));
+  ON_CALL(*right_motor_driver_, ReadMotorDriverState())
+    .WillByDefault(::testing::Return(
+      panther_hardware_interfaces::MotorDriverState({r_pos, r_vel, r_current, {0, 0}, {0, 0}})));
+
+  robot_driver_->UpdateMotorsState();
+
+  const auto & left = robot_driver_->GetData(kDriverName).GetMotorState(LynxMotorChannel::LEFT);
+  const auto & right =
+    robot_driver_->GetData(kDriverName).GetMotorState(LynxMotorChannel::RIGHT);
+
+  EXPECT_FLOAT_EQ(left.GetPosition(), l_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(left.GetVelocity(), l_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(left.GetTorque(), l_current * kRbtqCurrentFbToNewtonMeters);
+
+  EXPECT_FLOAT_EQ(right.GetPosition(), r_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(right.GetVelocity(), r_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(right.GetTorque(), r_current * kRbtqCurrentFbToNewtonMeters);
+}
+
+TEST_F(TestLynxRobotDriver, UpdateMotorsStateTimestamps)
+{
+  auto read_motor_driver_state_method = []() {
+    panther_hardware_interfaces::MotorDriverState state;
+    clock_gettime(CLOCK_MONOTONIC, &state.pos_timestamp);
+    clock_gettime(CLOCK_MONOTONIC, &state.vel_current_timestamp);
+    return state;
+  };
+
+  ON_CALL(*left_motor_driver_, ReadMotorDriverState())
+    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
+  ON_CALL(*right_motor_driver_, ReadMotorDriverState())
+    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
+
+  robot_driver_->UpdateMotorsState();
+
+  // sleep for timeout and check if timestamps were updated correctly
+  std::this_thread::sleep_for(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
+    std::chrono::milliseconds(10));
+
+  robot_driver_->UpdateMotorsState();
+
+  EXPECT_FALSE(robot_driver_->GetData(kDriverName).IsMotorStatesDataTimedOut());
+}
+
+TEST_F(TestLynxRobotDriver, UpdateMotorsStateTimeout)
+{
+  timespec current_time;
+  clock_gettime(CLOCK_MONOTONIC, &current_time);
+
+  panther_hardware_interfaces::MotorDriverState state = {0, 0, 0, current_time, current_time};
+
+  ON_CALL(*left_motor_driver_, ReadMotorDriverState()).WillByDefault(::testing::Return(state));
+  ON_CALL(*right_motor_driver_, ReadMotorDriverState()).WillByDefault(::testing::Return(state));
+
+  // sleep for pdo_motor_states_timeout_ms + 10ms
+  std::this_thread::sleep_for(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
+    std::chrono::milliseconds(10));
+
+  robot_driver_->UpdateMotorsState();
+
+  EXPECT_TRUE(robot_driver_->GetData(kDriverName).IsMotorStatesDataTimedOut());
+  EXPECT_TRUE(robot_driver_->GetData(kDriverName).IsError());
+}
+
+TEST_F(TestLynxRobotDriver, UpdateDriverState)
+{
+  using panther_hardware_interfaces::LynxMotorChannel;
+
+  const std::int16_t f_temp = 30;
+  const std::int16_t f_heatsink_temp = 31;
+  const std::uint16_t f_volt = 400;
+  const std::int16_t f_battery_current_1 = 10;
+  const std::int16_t f_battery_current_2 = 30;
+
+  const std::uint8_t fault_flag_overheat = static_cast<std::uint8_t>(0b01);
+  const std::uint8_t script_flag_encoder_disconnected = static_cast<std::uint8_t>(0b10);
+  const std::uint8_t runtime_error_loop_error = static_cast<std::uint8_t>(0b100);
+  const std::uint8_t runtime_error_safety_stop_active = static_cast<std::uint8_t>(0b1000);
+
+  ON_CALL(*driver_mock_, ReadDriverState())
+    .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
+      {fault_flag_overheat,
+       script_flag_encoder_disconnected,
+       runtime_error_loop_error,
+       runtime_error_safety_stop_active,
+       f_battery_current_1,
+       f_battery_current_2,
+       f_volt,
+       f_temp,
+       f_heatsink_temp,
+       {0, 0},
+       {0, 0}})));
+
+  robot_driver_->UpdateDriversState();
+
+  const auto & data = robot_driver_->GetData(kDriverName);
+  const auto & driver_state = robot_driver_->GetData(kDriverName).GetDriverState();
+
+  EXPECT_EQ(static_cast<std::int16_t>(driver_state.GetTemperature()), f_temp);
+  EXPECT_EQ(static_cast<std::int16_t>(driver_state.GetHeatsinkTemperature()), f_heatsink_temp);
+  EXPECT_EQ(static_cast<std::uint16_t>(driver_state.GetVoltage() * 10.0), f_volt);
+  EXPECT_EQ(
+    static_cast<std::int16_t>(driver_state.GetCurrent() * 10.0),
+    f_battery_current_1 + f_battery_current_2);
+
+  EXPECT_TRUE(data.GetFaultFlag().GetMessage().overheat);
+  EXPECT_TRUE(data.GetScriptFlag().GetMessage().encoder_disconnected);
+  EXPECT_TRUE(data.GetRuntimeError(LynxMotorChannel::RIGHT).GetMessage().loop_error);
+  EXPECT_TRUE(data.GetRuntimeError(LynxMotorChannel::LEFT).GetMessage().safety_stop_active);
+}
+
+TEST_F(TestLynxRobotDriver, UpdateDriverStateTimestamps)
+{
+  auto read_driver_state_method = []() {
+    panther_hardware_interfaces::DriverState state;
+    clock_gettime(CLOCK_MONOTONIC, &state.flags_current_timestamp);
+    clock_gettime(CLOCK_MONOTONIC, &state.voltages_temps_timestamp);
+    return state;
+  };
+
+  ON_CALL(*driver_mock_, ReadDriverState())
+    .WillByDefault(::testing::Invoke(read_driver_state_method));
+
+  robot_driver_->UpdateDriversState();
+
+  // sleep for timeout and check if timestamps were updated correctly
+  std::this_thread::sleep_for(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
+    std::chrono::milliseconds(10));
+
+  robot_driver_->UpdateDriversState();
+
+  EXPECT_FALSE(robot_driver_->GetData(kDriverName).IsDriverStateDataTimedOut());
+}
+
+TEST_F(TestLynxRobotDriver, UpdateDriverStateTimeout)
+{
+  timespec current_time;
+  clock_gettime(CLOCK_MONOTONIC, &current_time);
+
+  panther_hardware_interfaces::DriverState state = {
+    0, 0, 0, 0, 0, 0, 0, 0, 0, current_time, current_time};
+
+  ON_CALL(*driver_mock_, ReadDriverState()).WillByDefault(::testing::Return(state));
+
+  // sleep for pdo_driver_state_timeout_ms + 10ms
+  std::this_thread::sleep_for(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
+    std::chrono::milliseconds(10));
+
+  robot_driver_->UpdateDriversState();
+
+  EXPECT_TRUE(robot_driver_->GetData(kDriverName).IsDriverStateDataTimedOut());
+  EXPECT_TRUE(robot_driver_->GetData(kDriverName).IsError());
+}
+
+TEST_F(TestLynxRobotDriver, SendSpeedCommands)
+{
+  using panther_hardware_interfaces_test::kRadPerSecToRbtqCmd;
+
+  const float l_v = 0.1;
+  const float r_v = 0.2;
+
+  EXPECT_CALL(
+    *left_motor_driver_,
+    SendCmdVel(::testing::Eq(static_cast<std::int32_t>(l_v * kRadPerSecToRbtqCmd))))
+    .Times(1);
+  EXPECT_CALL(
+    *right_motor_driver_,
+    SendCmdVel(::testing::Eq(static_cast<std::int32_t>(r_v * kRadPerSecToRbtqCmd))))
+    .Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->SendSpeedCommands(l_v, r_v, 0, 0));
+}
+
+TEST_F(TestLynxRobotDriver, TurnOnEStop)
+{
+  EXPECT_CALL(*driver_mock_, TurnOnEStop()).Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->TurnOnEStop());
+}
+
+TEST_F(TestLynxRobotDriver, TurnOffEStop)
+{
+  EXPECT_CALL(*driver_mock_, TurnOffEStop()).Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->TurnOffEStop());
+}
+
+TEST_F(TestLynxRobotDriver, TurnOnEStopError)
+{
+  EXPECT_CALL(*driver_mock_, TurnOnEStop()).WillOnce(::testing::Throw(std::runtime_error("")));
+
+  ASSERT_THROW(robot_driver_->TurnOnEStop(), std::runtime_error);
+}
+
+TEST_F(TestLynxRobotDriver, TurnOffEStopError)
+{
+  EXPECT_CALL(*driver_mock_, TurnOffEStop()).WillOnce(::testing::Throw(std::runtime_error("")));
+
+  ASSERT_THROW(robot_driver_->TurnOffEStop(), std::runtime_error);
+}
+
+TEST_F(TestLynxRobotDriver, SafetyStop)
+{
+  EXPECT_CALL(*left_motor_driver_, TurnOnSafetyStop()).Times(1);
+  EXPECT_CALL(*right_motor_driver_, TurnOnSafetyStop()).Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->TurnOnSafetyStop());
+}
+
+TEST_F(TestLynxRobotDriver, SafetyStopError)
+{
+  EXPECT_CALL(*left_motor_driver_, TurnOnSafetyStop())
+    .WillOnce(::testing::Throw(std::runtime_error("")));
+  EXPECT_CALL(*right_motor_driver_, TurnOnSafetyStop()).Times(0);
+
+  ASSERT_THROW(robot_driver_->TurnOnSafetyStop(), std::runtime_error);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

From 6b465b1a65644a6c4e3d4815bd6a47c0db57a111 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Tue, 27 Aug 2024 13:58:43 +0000
Subject: [PATCH 028/100] move panther robot driver to separate file

---
 panther_hardware_interfaces/CMakeLists.txt    |   3 +-
 .../robot_driver/lynx_robot_driver.hpp        |   1 +
 .../robot_driver/panther_robot_driver.hpp     | 186 ++++++++++++++++++
 .../robot_driver/robot_driver.hpp             | 159 +--------------
 ...ot_driver.cpp => panther_robot_driver.cpp} |   1 +
 .../test_panther_robot_driver.cpp             |   1 +
 6 files changed, 191 insertions(+), 160 deletions(-)
 create mode 100644 panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
 rename panther_hardware_interfaces/src/panther_system/robot_driver/{robot_driver.cpp => panther_robot_driver.cpp} (99%)

diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt
index 5b646f67..681f4287 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/panther_hardware_interfaces/CMakeLists.txt
@@ -152,7 +152,7 @@ if(BUILD_TESTING)
     src/panther_system/robot_driver/canopen_manager.cpp
     src/panther_system/robot_driver/roboteq_driver.cpp
     src/panther_system/robot_driver/roboteq_data_converters.cpp
-    src/panther_system/robot_driver/robot_driver.cpp
+    src/panther_system/robot_driver/panther_robot_driver.cpp
     src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_panther_robot_driver
@@ -169,7 +169,6 @@ if(BUILD_TESTING)
     src/panther_system/robot_driver/canopen_manager.cpp
     src/panther_system/robot_driver/roboteq_driver.cpp
     src/panther_system/robot_driver/roboteq_data_converters.cpp
-    src/panther_system/robot_driver/robot_driver.cpp
     src/panther_system/robot_driver/lynx_robot_driver.cpp
     src/utils.cpp)
   target_include_directories(
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
index 359cb91e..51211bc6 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
@@ -16,6 +16,7 @@
 #define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_LYNX_ROBOT_DRIVER_HPP_
 
 #include <chrono>
+#include <string>
 
 #include <panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp>
 #include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
new file mode 100644
index 00000000..881e464b
--- /dev/null
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
@@ -0,0 +1,186 @@
+// 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_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_PANTHER_ROBOT_DRIVER_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_PANTHER_ROBOT_DRIVER_HPP_
+
+#include <chrono>
+#include <string>
+
+#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
+
+namespace panther_hardware_interfaces
+{
+
+struct PantherMotorNames
+{
+  static constexpr char LEFT[] = "left";
+  static constexpr char RIGHT[] = "right";
+};
+
+struct PantherDriverNames
+{
+  static constexpr char FRONT[] = "front";
+  static constexpr char REAR[] = "rear";
+};
+
+struct PantherMotorChannel
+{
+  static constexpr std::uint8_t LEFT = RoboteqDriver::kChannel2;
+  static constexpr std::uint8_t RIGHT = RoboteqDriver::kChannel1;
+};
+
+/**
+ * @brief This class abstracts the usage of two Roboteq controllers.
+ * It uses canopen_manager for communication with Roboteq controllers,
+ * implements the activation procedure for controllers (resets script and sends initial 0 command),
+ * and provides methods to get data feedback and send commands.
+ * Data is converted between raw Roboteq formats and SI units using roboteq_data_converters.
+ */
+class PantherRobotDriver : public RobotDriver
+{
+public:
+  PantherRobotDriver(
+    const std::shared_ptr<Driver> front_driver, const std::shared_ptr<Driver> rear_driver,
+    const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings,
+    const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000));
+
+  ~PantherRobotDriver()
+  {
+    front_driver_.reset();
+    rear_driver_.reset();
+    Deinitialize();
+  };
+
+  /**
+   * @brief Starts CAN communication and waits for boot to finish
+   *
+   * @exception std::runtime_error if boot fails
+   */
+  void Initialize() override;
+
+  /**
+   * @brief Deinitialize can communication
+   */
+  void Deinitialize() override;
+
+  /**
+   * @brief Activate procedure for Roboteq drivers - reset scripts and send 0 commands on both
+   * channels. Blocking function, takes around 2 seconds to finish
+   *
+   * @exception std::runtime_error if any procedure step fails
+   */
+  void Activate() override;
+
+  /**
+   * @brief Updates current communication state with Roboteq drivers
+   *
+   * @exception std::runtime_error if CAN error was detected
+   */
+  void UpdateCommunicationState() override;
+
+  /**
+   * @brief Updates current motors' state (position, velocity, current).
+   *
+   * @exception std::runtime_error if CAN error was detected
+   */
+  void UpdateMotorsState() override;
+
+  /**
+   * @brief Updates current Roboteq driver state (flags, temperatures, voltage, battery current)
+   *
+   * @exception std::runtime_error if CAN error was detected
+   */
+  void UpdateDriversState() override;
+
+  /**
+   * @brief Get data feedback from the driver
+   *
+   * @param name name of the data to get
+   *
+   * @return data feedback
+   * @exception std::runtime_error if data with the given name does not exist
+   */
+  const RoboteqData & GetData(const std::string & name) override;
+
+  /**
+   * @brief Write speed commands to motors
+   *
+   * @param speed_fl front left motor speed in rad/s
+   * @param speed_fr front right motor speed in rad/s
+   * @param speed_rl rear left motor speed in rad/s
+   * @param speed_rr rear right motor speed in rad/s
+   *
+   * @exception std::runtime_error if send command fails or CAN error was detected
+   */
+  void SendSpeedCommands(
+    const float speed_fl, const float speed_fr, const float speed_rl, const float speed_rr);
+
+  /**
+   * @brief Turns on Roboteq E-stop
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  void TurnOnEStop();
+
+  /**
+   * @brief Turns off Roboteq E-stop
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  void TurnOffEStop();
+
+  /**
+   * @brief Turns on Roboteq safety stop. To turn it off, it is necessary to send
+   * 0 commands to motors.
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  void TurnOnSafetyStop();
+
+  /**
+   * @brief Attempt to clear driver error flags by sending 0 velocity commands to motors. If Roboteq
+   * driver faults still exist, the error flag will remain active.
+   */
+  inline void AttemptErrorFlagResetWithZeroSpeed() { SendSpeedCommands(0.0, 0.0, 0.0, 0.0); };
+
+private:
+  void SetMotorsStates(
+    RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
+    const timespec & current_time);
+  void SetDriverState(RoboteqData & data, const DriverState & state, const timespec & current_time);
+
+  bool initialized_ = false;
+
+  CANopenManager canopen_manager_;
+
+  std::shared_ptr<Driver> front_driver_;
+  std::shared_ptr<Driver> rear_driver_;
+
+  RoboteqData front_data_;
+  RoboteqData rear_data_;
+
+  RoboteqVelocityCommandConverter roboteq_vel_cmd_converter_;
+
+  const std::chrono::milliseconds pdo_motor_states_timeout_ms_;
+  const std::chrono::milliseconds pdo_driver_state_timeout_ms_;
+  const std::chrono::milliseconds activate_wait_time_;
+};
+
+}  // namespace panther_hardware_interfaces
+
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_PANTHER_ROBOT_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
index 4db34f3c..b972f33b 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
@@ -15,33 +15,13 @@
 #ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_
 #define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_
 
-#include <chrono>
+#include <string>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
 #include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
 
 namespace panther_hardware_interfaces
 {
 
-struct PantherMotorNames
-{
-  static constexpr char LEFT[] = "left";
-  static constexpr char RIGHT[] = "right";
-};
-
-struct PantherDriverNames
-{
-  static constexpr char FRONT[] = "front";
-  static constexpr char REAR[] = "rear";
-};
-
-struct PantherMotorChannel
-{
-  static constexpr std::uint8_t LEFT = RoboteqDriver::kChannel2;
-  static constexpr std::uint8_t RIGHT = RoboteqDriver::kChannel1;
-};
-
 /**
  * @brief Abstract class for managing robot drivers.
  */
@@ -140,143 +120,6 @@ class RobotDriver
   inline void AttemptErrorFlagResetWithZeroSpeed() { SendSpeedCommands(0.0, 0.0, 0.0, 0.0); };
 };
 
-/**
- * @brief This class abstracts the usage of two Roboteq controllers.
- * It uses canopen_manager for communication with Roboteq controllers,
- * implements the activation procedure for controllers (resets script and sends initial 0 command),
- * and provides methods to get data feedback and send commands.
- * Data is converted between raw Roboteq formats and SI units using roboteq_data_converters.
- */
-class PantherRobotDriver : public RobotDriver
-{
-public:
-  PantherRobotDriver(
-    const std::shared_ptr<Driver> front_driver, const std::shared_ptr<Driver> rear_driver,
-    const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings,
-    const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000));
-
-  ~PantherRobotDriver()
-  {
-    front_driver_.reset();
-    rear_driver_.reset();
-    Deinitialize();
-  };
-
-  /**
-   * @brief Starts CAN communication and waits for boot to finish
-   *
-   * @exception std::runtime_error if boot fails
-   */
-  void Initialize() override;
-
-  /**
-   * @brief Deinitialize can communication
-   */
-  void Deinitialize() override;
-
-  /**
-   * @brief Activate procedure for Roboteq drivers - reset scripts and send 0 commands on both
-   * channels. Blocking function, takes around 2 seconds to finish
-   *
-   * @exception std::runtime_error if any procedure step fails
-   */
-  void Activate() override;
-
-  /**
-   * @brief Updates current communication state with Roboteq drivers
-   *
-   * @exception std::runtime_error if CAN error was detected
-   */
-  void UpdateCommunicationState() override;
-
-  /**
-   * @brief Updates current motors' state (position, velocity, current).
-   *
-   * @exception std::runtime_error if CAN error was detected
-   */
-  void UpdateMotorsState() override;
-
-  /**
-   * @brief Updates current Roboteq driver state (flags, temperatures, voltage, battery current)
-   *
-   * @exception std::runtime_error if CAN error was detected
-   */
-  void UpdateDriversState() override;
-
-  /**
-   * @brief Get data feedback from the driver
-   *
-   * @param name name of the data to get
-   *
-   * @return data feedback
-   * @exception std::runtime_error if data with the given name does not exist
-   */
-  const RoboteqData & GetData(const std::string & name) override;
-
-  /**
-   * @brief Write speed commands to motors
-   *
-   * @param speed_fl front left motor speed in rad/s
-   * @param speed_fr front right motor speed in rad/s
-   * @param speed_rl rear left motor speed in rad/s
-   * @param speed_rr rear right motor speed in rad/s
-   *
-   * @exception std::runtime_error if send command fails or CAN error was detected
-   */
-  void SendSpeedCommands(
-    const float speed_fl, const float speed_fr, const float speed_rl, const float speed_rr);
-
-  /**
-   * @brief Turns on Roboteq E-stop
-   *
-   * @exception std::runtime_error if any operation returns error
-   */
-  void TurnOnEStop();
-
-  /**
-   * @brief Turns off Roboteq E-stop
-   *
-   * @exception std::runtime_error if any operation returns error
-   */
-  void TurnOffEStop();
-
-  /**
-   * @brief Turns on Roboteq safety stop. To turn it off, it is necessary to send
-   * 0 commands to motors.
-   *
-   * @exception std::runtime_error if any operation returns error
-   */
-  void TurnOnSafetyStop();
-
-  /**
-   * @brief Attempt to clear driver error flags by sending 0 velocity commands to motors. If Roboteq
-   * driver faults still exist, the error flag will remain active.
-   */
-  inline void AttemptErrorFlagResetWithZeroSpeed() { SendSpeedCommands(0.0, 0.0, 0.0, 0.0); };
-
-private:
-  void SetMotorsStates(
-    RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
-    const timespec & current_time);
-  void SetDriverState(RoboteqData & data, const DriverState & state, const timespec & current_time);
-
-  bool initialized_ = false;
-
-  CANopenManager canopen_manager_;
-
-  std::shared_ptr<Driver> front_driver_;
-  std::shared_ptr<Driver> rear_driver_;
-
-  RoboteqData front_data_;
-  RoboteqData rear_data_;
-
-  RoboteqVelocityCommandConverter roboteq_vel_cmd_converter_;
-
-  const std::chrono::milliseconds pdo_motor_states_timeout_ms_;
-  const std::chrono::milliseconds pdo_driver_state_timeout_ms_;
-  const std::chrono::milliseconds activate_wait_time_;
-};
-
 }  // namespace panther_hardware_interfaces
 
 #endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp
similarity index 99%
rename from panther_hardware_interfaces/src/panther_system/robot_driver/robot_driver.cpp
rename to panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp
index d1e4cac9..5e09ad8e 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/robot_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp
@@ -25,6 +25,7 @@
 #include "lely/util/chrono.hpp"
 
 #include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp"
 #include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
 #include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
 
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
index 30a8aa3d..cc0da7d0 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
@@ -24,6 +24,7 @@
 
 #include <panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp>
 #include <panther_hardware_interfaces/panther_system/robot_driver/driver.hpp>
+#include <panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp>
 #include <panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp>
 
 #include "utils/fake_can_socket.hpp"

From 670137f001202a8e439794bbbb386ad32f66578f Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 30 Aug 2024 14:24:59 +0200
Subject: [PATCH 029/100] Save work

---
 lynx_description/config/WH05.yaml                 |  1 +
 lynx_description/urdf/gazebo.urdf.xacro           |  3 +++
 lynx_description/urdf/lynx_macro.urdf.xacro       |  7 ++++---
 panther_description/urdf/panther_macro.urdf.xacro |  4 ++--
 panther_gazebo/launch/simulate_robot.launch.py    | 10 ++++++++++
 panther_gazebo/launch/spawn_robot.launch.py       | 15 ++++++++++++++-
 6 files changed, 34 insertions(+), 6 deletions(-)

diff --git a/lynx_description/config/WH05.yaml b/lynx_description/config/WH05.yaml
index 01b4967f..fb49e17e 100644
--- a/lynx_description/config/WH05.yaml
+++ b/lynx_description/config/WH05.yaml
@@ -7,3 +7,4 @@ inertia: { ixx: 0.014738, iyy: 0.0261, izz: 0.014738 }
 inertia_y_offset: 0.0
 mesh_package: lynx_description
 folder_path: meshes/WH05
+mecanum: False
diff --git a/lynx_description/urdf/gazebo.urdf.xacro b/lynx_description/urdf/gazebo.urdf.xacro
index b8b2afdb..6eea0941 100644
--- a/lynx_description/urdf/gazebo.urdf.xacro
+++ b/lynx_description/urdf/gazebo.urdf.xacro
@@ -42,6 +42,9 @@
         <parameters>${config_file}</parameters>
         <ros>
           <namespace>${namespace}</namespace>
+          <remapping>gz_ros2_control/e_stop:=hardware/e_stop</remapping>
+          <remapping>gz_ros2_control/e_stop_reset:=hardware/e_stop_reset</remapping>
+          <remapping>gz_ros2_control/e_stop_trigger:=hardware/e_stop_trigger</remapping>
           <remapping>imu_broadcaster/imu:=imu/data</remapping>
           <remapping>drive_controller/cmd_vel_unstamped:=cmd_vel</remapping>
           <remapping>drive_controller/odom:=odometry/wheels</remapping>
diff --git a/lynx_description/urdf/lynx_macro.urdf.xacro b/lynx_description/urdf/lynx_macro.urdf.xacro
index 94b691ae..8443fe58 100644
--- a/lynx_description/urdf/lynx_macro.urdf.xacro
+++ b/lynx_description/urdf/lynx_macro.urdf.xacro
@@ -57,10 +57,11 @@
     <ros2_control name="${ns}lynx_system" type="system">
       <hardware>
         <xacro:if value="${use_sim}">
-          <plugin>ign_ros2_control/IgnitionSystem</plugin>
+          <plugin>panther_gazebo/GzPantherSystem</plugin>
+          <param name="e_stop_initial_state">true</param>
         </xacro:if>
 
-        <xacro:unless value="$(arg use_sim)">
+        <xacro:unless value="${use_sim}">
           <plugin>panther_hardware_interfaces/PantherSystem</plugin>
 
           <param name="encoder_resolution">1600</param>
@@ -140,7 +141,7 @@
       </xacro:if>
     </ros2_control>
 
-    <xacro:unless value="$(arg use_sim)">
+    <xacro:unless value="${use_sim}">
       <ros2_control name="${ns}imu" type="sensor">
         <hardware>
           <plugin>panther_hardware_interfaces/PantherImuSensor</plugin>
diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro
index 8d2a8320..4911f276 100644
--- a/panther_description/urdf/panther_macro.urdf.xacro
+++ b/panther_description/urdf/panther_macro.urdf.xacro
@@ -61,7 +61,7 @@
           <param name="e_stop_initial_state">true</param>
         </xacro:if>
 
-        <xacro:unless value="$(arg use_sim)">
+        <xacro:unless value="${use_sim}">
           <plugin>panther_hardware_interfaces/PantherSystem</plugin>
 
           <param name="panther_version">${panther_version}</param>
@@ -143,7 +143,7 @@
       </xacro:if>
     </ros2_control>
 
-    <xacro:unless value="$(arg use_sim)">
+    <xacro:unless value="${use_sim}">
       <ros2_control name="${ns}imu" type="sensor">
         <hardware>
           <plugin>panther_hardware_interfaces/PantherImuSensor</plugin>
diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py
index ad7252e7..90435166 100644
--- a/panther_gazebo/launch/simulate_robot.launch.py
+++ b/panther_gazebo/launch/simulate_robot.launch.py
@@ -72,6 +72,14 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
+    robot = LaunchConfiguration("robot")
+    declare_robot_arg = DeclareLaunchArgument(
+        "robot",
+        default_value=EnvironmentVariable("ROBOT_MODEL", default_value="panther"),
+        description="Specify robot model.",
+        choices=["panther", "lynx"],
+    )
+
     use_ekf = LaunchConfiguration("use_ekf")
     declare_use_ekf_arg = DeclareLaunchArgument(
         "use_ekf",
@@ -89,6 +97,7 @@ def generate_launch_description():
         launch_arguments={
             "add_wheel_joints": "False",
             "namespace": namespace,
+            "robot": robot,
             "use_sim": "True",
         }.items(),
     )
@@ -191,6 +200,7 @@ def generate_launch_description():
             declare_components_config_path_arg,
             declare_gz_bridge_config_path_arg,
             declare_namespace_arg,
+            declare_robot_arg,
             declare_use_ekf_arg,
             SetUseSimTime(True),
             spawn_robot_launch,
diff --git a/panther_gazebo/launch/spawn_robot.launch.py b/panther_gazebo/launch/spawn_robot.launch.py
index 42e3e27e..945f92a3 100644
--- a/panther_gazebo/launch/spawn_robot.launch.py
+++ b/panther_gazebo/launch/spawn_robot.launch.py
@@ -21,6 +21,7 @@
     EnvironmentVariable,
     LaunchConfiguration,
     PathJoinSubstitution,
+    PythonExpression,
 )
 from launch_ros.actions import Node, SetUseSimTime
 from launch_ros.substitutions import FindPackageShare
@@ -36,6 +37,14 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
+    robot = LaunchConfiguration("robot")
+    declare_robot_arg = DeclareLaunchArgument(
+        "robot",
+        default_value=EnvironmentVariable("ROBOT_MODEL", default_value="panther"),
+        description="Specify robot model.",
+        choices=["panther", "lynx"],
+    )
+
     x = LaunchConfiguration("x")
     declare_x_arg = DeclareLaunchArgument(
         "x", default_value="0.0", description="Initial robot position in the global 'x' axis."
@@ -72,16 +81,19 @@ def generate_launch_description():
     }
     welcome_msg = welcomeMsg("---", "simulation", log_stats)
 
+    urdf_packages = PythonExpression(["'", robot, "_description'"])
     add_wheel_joints = LaunchConfiguration("add_wheel_joints", default="True")
+
     load_urdf = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare("panther_description"), "launch", "load_urdf.launch.py"]
+                [FindPackageShare(urdf_packages), "launch", "load_urdf.launch.py"]
             )
         ),
         launch_arguments={
             "add_wheel_joints": add_wheel_joints,
             "namespace": namespace,
+            "robot": robot,
             "use_sim": "True",
         }.items(),
     )
@@ -114,6 +126,7 @@ def generate_launch_description():
     return LaunchDescription(
         [
             declare_namespace_arg,
+            declare_robot_arg,
             declare_x_arg,
             declare_y_arg,
             declare_z_arg,

From 0c4ea8576660b2508e35cecf52e1b2b7bdac9970 Mon Sep 17 00:00:00 2001
From: Dawid Kmak <73443304+KmakD@users.noreply.github.com>
Date: Mon, 2 Sep 2024 14:17:06 +0200
Subject: [PATCH 030/100] ROS 2 canopen manager and roboteq driver refactor
 (#396)

* roboteq driver abstract class

* canopen controller abstraction

* simplify canopen controller

* fix make

* update roboteq_driver tests

* move test files

* update roboteq driver

* rename canopen controller to manager

* small fixes

* add roboteq driver boot tests

* update roboteq tests

* review fixes

* mock first

* add canopen activation method

* update names
---
 panther_hardware_interfaces/CMakeLists.txt    |  20 +-
 ...pen_controller.hpp => canopen_manager.hpp} |  55 ++-
 .../motors_controller/driver.hpp              | 150 +++++++
 .../roboteq_data_converters.hpp               |  16 +-
 .../motors_controller/roboteq_driver.hpp      | 149 ++++---
 panther_hardware_interfaces/package.xml       |   5 +-
 ...pen_controller.cpp => canopen_manager.cpp} |  95 ++---
 .../roboteq_data_converters.cpp               |   4 +-
 .../motors_controller/roboteq_driver.cpp      | 151 +++----
 .../test/config/canopen_configuration.yaml    |   5 -
 .../test/config/slave_2.bin                   | Bin 13 -> 0 bytes
 .../test_canopen_controller.cpp               | 101 -----
 .../motors_controller/test_roboteq_driver.cpp | 390 ------------------
 .../test_canopen_manager.cpp                  | 101 +++++
 .../motors_controller/test_roboteq_driver.cpp | 390 ++++++++++++++++++
 .../test/utils/fake_can_socket.hpp            |  80 ++++
 .../{roboteqs_mock.hpp => mock_roboteq.hpp}   |  66 ++-
 .../test/utils/test_constants.hpp             |   2 +-
 18 files changed, 984 insertions(+), 796 deletions(-)
 rename panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/{canopen_controller.hpp => canopen_manager.hpp} (75%)
 create mode 100644 panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/driver.hpp
 rename panther_hardware_interfaces/src/panther_system/motors_controller/{canopen_controller.cpp => canopen_manager.cpp} (63%)
 delete mode 100644 panther_hardware_interfaces/test/config/slave_2.bin
 delete mode 100644 panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp
 delete mode 100644 panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_driver.cpp
 create mode 100644 panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp
 create mode 100644 panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
 create mode 100644 panther_hardware_interfaces/test/utils/fake_can_socket.hpp
 rename panther_hardware_interfaces/test/utils/{roboteqs_mock.hpp => mock_roboteq.hpp} (85%)

diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt
index 8713f699..b87fda02 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/panther_hardware_interfaces/CMakeLists.txt
@@ -55,7 +55,7 @@ add_library(
   src/panther_imu_sensor/panther_imu_sensor.cpp
   src/panther_system/gpio/gpio_controller.cpp
   src/panther_system/gpio/gpio_driver.cpp
-  src/panther_system/motors_controller/canopen_controller.cpp
+  src/panther_system/motors_controller/canopen_manager.cpp
   src/panther_system/motors_controller/motors_controller.cpp
   src/panther_system/motors_controller/roboteq_data_converters.cpp
   src/panther_system/motors_controller/roboteq_driver.cpp
@@ -118,24 +118,24 @@ if(BUILD_TESTING)
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gtest(
-    ${PROJECT_NAME}_test_canopen_controller
-    test/panther_system/motors_controller/test_canopen_controller.cpp
-    src/panther_system/motors_controller/canopen_controller.cpp
+    ${PROJECT_NAME}_test_canopen_manager
+    test/unit/panther_system/motors_controller/test_canopen_manager.cpp
+    src/panther_system/motors_controller/canopen_manager.cpp
     src/panther_system/motors_controller/roboteq_driver.cpp
     src/utils.cpp)
   target_include_directories(
-    ${PROJECT_NAME}_test_canopen_controller
+    ${PROJECT_NAME}_test_canopen_manager
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
-  ament_target_dependencies(${PROJECT_NAME}_test_canopen_controller rclcpp
+  ament_target_dependencies(${PROJECT_NAME}_test_canopen_manager rclcpp
                             panther_msgs panther_utils)
-  target_link_libraries(${PROJECT_NAME}_test_canopen_controller
+  target_link_libraries(${PROJECT_NAME}_test_canopen_manager
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_driver
-    test/panther_system/motors_controller/test_roboteq_driver.cpp
-    src/panther_system/motors_controller/canopen_controller.cpp
+    test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
+    src/panther_system/motors_controller/canopen_manager.cpp
     src/panther_system/motors_controller/roboteq_driver.cpp
     src/utils.cpp)
   target_include_directories(
@@ -150,7 +150,7 @@ if(BUILD_TESTING)
   ament_add_gtest(
     ${PROJECT_NAME}_test_motors_controller
     test/panther_system/motors_controller/test_motors_controller.cpp
-    src/panther_system/motors_controller/canopen_controller.cpp
+    src/panther_system/motors_controller/canopen_manager.cpp
     src/panther_system/motors_controller/roboteq_driver.cpp
     src/panther_system/motors_controller/roboteq_data_converters.cpp
     src/panther_system/motors_controller/motors_controller.cpp
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp
similarity index 75%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp
rename to panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp
index 883d7363..d1b924a5 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_CONTROLLER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_CONTROLLER_HPP_
+#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_MANAGER_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_MANAGER_HPP_
 
 #include <atomic>
 #include <chrono>
@@ -33,11 +33,15 @@
 #include "lely/io2/sys/sigset.hpp"
 #include "lely/io2/sys/timer.hpp"
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp"
-
 namespace panther_hardware_interfaces
 {
 
+struct CANopenObject
+{
+  const std::uint16_t id;
+  const std::uint8_t subid;
+};
+
 struct CANopenSettings
 {
   std::string can_interface_name;
@@ -52,15 +56,15 @@ struct CANopenSettings
 };
 
 /**
- * @brief CANopenController takes care of CANopen communication - creates master controller
+ * @brief CANopenManager takes care of CANopen communication - creates master controller
  * and two Roboteq drivers (front and rear)
  */
-class CANopenController
+class CANopenManager
 {
 public:
-  CANopenController(const CANopenSettings & canopen_settings);
+  CANopenManager(const CANopenSettings & canopen_settings);
 
-  ~CANopenController() { Deinitialize(); }
+  ~CANopenManager() { Deinitialize(); }
 
   /**
    * @brief Starts CANopen communication (in a new thread) and waits for boot to finish
@@ -74,8 +78,27 @@ class CANopenController
    */
   void Deinitialize();
 
-  std::shared_ptr<RoboteqDriver> GetFrontDriver() { return front_driver_; }
-  std::shared_ptr<RoboteqDriver> GetRearDriver() { return rear_driver_; }
+  /**
+   * @brief Activates CANopen communication thread. This method should be invoked after all objects
+   * using this communication are created.
+   *
+   * @exception std::runtime_error if CAN communication not activated or not initialized
+   */
+  void Activate();
+
+  /**
+   * @brief Returns master controller
+   *
+   * @return std::shared_ptr<lely::canopen::AsyncMaster> master controller
+   * @exception std::runtime_error if CANopenManager is not initialized
+   */
+  std::shared_ptr<lely::canopen::AsyncMaster> GetMaster()
+  {
+    if (!initialized_) {
+      throw std::runtime_error("CANopenManager not initialized.");
+    }
+    return master_;
+  }
 
 private:
   void InitializeCANCommunication();
@@ -88,13 +111,6 @@ class CANopenController
    */
   void NotifyCANCommunicationStarted(const bool result);
 
-  /**
-   * @brief Triggers boot on front and rear Roboteq drivers and waits for finish
-   *
-   * @exception std::runtime_error if boot fails
-   */
-  void BootDrivers();
-
   // Priority set to be higher than the priority of the main ros2 control node (50)
   static constexpr unsigned kCANopenThreadSchedPriority = 60;
 
@@ -115,12 +131,9 @@ class CANopenController
   std::shared_ptr<lely::io::CanChannel> chan_;
   std::shared_ptr<lely::canopen::AsyncMaster> master_;
 
-  std::shared_ptr<RoboteqDriver> front_driver_;
-  std::shared_ptr<RoboteqDriver> rear_driver_;
-
   const CANopenSettings canopen_settings_;
 };
 
 }  // namespace panther_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_CONTROLLER_HPP_
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_MANAGER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/driver.hpp
new file mode 100644
index 00000000..87c341d0
--- /dev/null
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/driver.hpp
@@ -0,0 +1,150 @@
+// 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_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_DRIVER_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_DRIVER_HPP_
+
+#include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <future>
+#include <memory>
+#include <mutex>
+#include <string>
+
+#include "lely/coapp/loop_driver.hpp"
+
+namespace panther_hardware_interfaces
+{
+
+struct MotorDriverState
+{
+  std::int32_t pos;
+  std::int16_t vel;
+  std::int16_t current;
+
+  timespec pos_timestamp;
+  timespec vel_current_timestamp;
+};
+
+struct DriverState
+{
+  std::uint8_t fault_flags;
+  std::uint8_t script_flags;
+  std::uint8_t runtime_stat_flag_motor_1;
+  std::uint8_t runtime_stat_flag_motor_2;
+
+  std::int16_t battery_current_1;
+  std::int16_t battery_current_2;
+
+  std::uint16_t battery_voltage;
+
+  std::int16_t mcu_temp;
+  std::int16_t heatsink_temp;
+
+  timespec flags_current_timestamp;
+  timespec voltages_temps_timestamp;
+};
+
+/**
+ * @brief Abstract class for motor driver
+ */
+class MotorDriver
+{
+public:
+  /**
+   * @brief Reads motors' state data returned from  (PDO 1 and 2) and saves
+   * last timestamps
+   */
+  virtual MotorDriverState ReadMotorDriverState() = 0;
+
+  /**
+   * @brief Sends commands to the motors
+   *
+   * @param cmd command value in the range [-1000, 1000]
+   *
+   * @exception std::runtime_error if operation fails
+   */
+  virtual void SendCmdVel(const std::int32_t cmd) = 0;
+
+  /**
+   * @brief Sends a safety stop command to the motor
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void TurnOnSafetyStop() = 0;
+};
+
+/**
+ * @brief Abstract class for  driver
+ */
+class Driver
+{
+public:
+  /**
+   * @brief Triggers boot operations
+   *
+   * @exception std::runtime_error if triggering boot fails
+   */
+  virtual std::future<void> Boot() = 0;
+
+  /**
+   * @brief Returns true if CAN error was detected.
+   */
+  virtual bool IsCANError() const = 0;
+
+  /**
+   * @brief Returns true if heartbeat timeout encountered.
+   */
+  virtual bool IsHeartbeatTimeout() const = 0;
+
+  /**
+   * @brief Reads driver state data returned from  (PDO 3 and 4): error flags, battery
+   * voltage, battery currents (for channel 1 and 2, they are not the same as motor currents),
+   * temperatures. Also saves the last timestamps
+   */
+  virtual DriverState ReadDriverState() = 0;
+
+  /**
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void ResetScript() = 0;
+
+  /**
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void TurnOnEStop() = 0;
+
+  /**
+   * @exception std::runtime_error if any operation returns error
+   */
+  virtual void TurnOffEStop() = 0;
+
+  /**
+   * @brief Adds a motor driver to the driver
+   */
+  virtual void AddMotorDriver(
+    const std::string name, std::shared_ptr<MotorDriver> motor_driver) = 0;
+
+  virtual std::shared_ptr<MotorDriver> GetMotorDriver(const std::string & name) = 0;
+
+  /**
+   * @brief Alias for a shared pointer to a Driver object.
+   */
+  using SharedPtr = std::shared_ptr<Driver>;
+};
+
+}  // namespace panther_hardware_interfaces
+
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp
index 0a167e7c..4a6d6b25 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp
@@ -71,7 +71,7 @@ class MotorState
 public:
   MotorState(const DrivetrainSettings & drivetrain_settings);
 
-  void SetData(const RoboteqMotorState & motor_state) { motor_state_ = motor_state; };
+  void SetData(const MotorDriverState & motor_state) { motor_state_ = motor_state; };
 
   float GetPosition() const { return motor_state_.pos * roboteq_pos_feedback_to_radians_; }
   float GetVelocity() const
@@ -88,7 +88,7 @@ class MotorState
   float roboteq_vel_feedback_to_radians_per_second_;
   float roboteq_current_feedback_to_newton_meters_;
 
-  RoboteqMotorState motor_state_ = {0, 0, 0};
+  MotorDriverState motor_state_ = {0, 0, 0, {0, 0}, {0, 0}};
 };
 
 /**
@@ -148,10 +148,10 @@ class RuntimeError : public FlagError
  * @brief Class for storing and converting the current state of the Roboteq drivers (temperature,
  * voltage and battery current)
  */
-class DriverState
+class RoboteqDriverState
 {
 public:
-  DriverState() {}
+  RoboteqDriverState() {}
 
   void SetTemperature(const std::int16_t temp) { temp_ = temp; };
   void SetHeatsinkTemperature(const std::int16_t heatsink_temp) { heatsink_temp_ = heatsink_temp; };
@@ -190,9 +190,9 @@ class RoboteqData
   }
 
   void SetMotorsStates(
-    const RoboteqMotorState & left_state, const RoboteqMotorState & right_state,
+    const MotorDriverState & left_state, const MotorDriverState & right_state,
     const bool data_timed_out);
-  void SetDriverState(const RoboteqDriverState & state, const bool data_timed_out);
+  void SetDriverState(const DriverState & state, const bool data_timed_out);
   void SetCANError(const bool can_error) { can_error_ = can_error; }
   void SetHeartbeatTimeout(const bool heartbeat_timeout) { heartbeat_timeout_ = heartbeat_timeout; }
 
@@ -210,7 +210,7 @@ class RoboteqData
 
   const MotorState & GetLeftMotorState() const { return left_motor_state_; }
   const MotorState & GetRightMotorState() const { return right_motor_state_; }
-  const DriverState & GetDriverState() const { return driver_state_; }
+  const RoboteqDriverState & GetDriverState() const { return driver_state_; }
 
   bool IsMotorStatesDataTimedOut() const { return motor_states_data_timed_out_; }
   bool IsDriverStateDataTimedOut() const { return driver_state_data_timed_out_; }
@@ -231,7 +231,7 @@ class RoboteqData
   MotorState left_motor_state_;
   MotorState right_motor_state_;
 
-  DriverState driver_state_;
+  RoboteqDriverState driver_state_;
 
   FaultFlag fault_flags_;
   ScriptFlag script_flags_;
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
index 498231c2..05859405 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp
@@ -25,48 +25,19 @@
 
 #include "lely/coapp/loop_driver.hpp"
 
-namespace panther_hardware_interfaces
-{
-
-struct RoboteqMotorState
-{
-  std::int32_t pos;
-  std::int16_t vel;
-  std::int16_t current;
-};
-
-struct RoboteqMotorsStates
-{
-  RoboteqMotorState motor_1;
-  RoboteqMotorState motor_2;
-
-  timespec pos_timestamp;
-  timespec vel_current_timestamp;
-};
+#include "panther_hardware_interfaces/panther_system/motors_controller/driver.hpp"
 
-struct RoboteqDriverState
+namespace panther_hardware_interfaces
 {
-  std::uint8_t fault_flags;
-  std::uint8_t script_flags;
-  std::uint8_t runtime_stat_flag_motor_1;
-  std::uint8_t runtime_stat_flag_motor_2;
-
-  std::int16_t battery_current_1;
-  std::int16_t battery_current_2;
-
-  std::uint16_t battery_voltage;
 
-  std::int16_t mcu_temp;
-  std::int16_t heatsink_temp;
-
-  timespec flags_current_timestamp;
-  timespec voltages_temps_timestamp;
-};
+// Forward declaration
+class RoboteqMotorDriver;
 
 /**
- * @brief Implementation of LoopDriver for Roboteq drivers
+ * @brief Hardware implementation of Driver with lely LoopDriver for Roboteq drivers
+ * control
  */
-class RoboteqDriver : public lely::canopen::LoopDriver
+class RoboteqDriver : public Driver, public lely::canopen::LoopDriver
 {
 public:
   RoboteqDriver(
@@ -78,70 +49,52 @@ class RoboteqDriver : public lely::canopen::LoopDriver
    *
    * @exception std::runtime_error if triggering boot fails
    */
-  std::future<void> Boot();
+  std::future<void> Boot() override;
 
   /**
    * @brief Returns true if CAN error was detected.
    */
-  bool IsCANError() const { return can_error_.load(); }
+  bool IsCANError() const override { return can_error_.load(); };
 
   /**
    * @brief Returns true if heartbeat timeout encountered.
    */
-  bool IsHeartbeatTimeout() const { return heartbeat_timeout_.load(); }
-
-  /**
-   * @brief Reads motors' state data returned from Roboteq (PDO 1 and 2) and saves
-   * last timestamps
-   */
-  RoboteqMotorsStates ReadRoboteqMotorsStates();
+  bool IsHeartbeatTimeout() const override { return heartbeat_timeout_.load(); };
 
   /**
    * @brief Reads driver state data returned from Roboteq (PDO 3 and 4): error flags, battery
    * voltage, battery currents (for channel 1 and 2, they are not the same as motor currents),
    * temperatures. Also saves the last timestamps
    */
-  RoboteqDriverState ReadRoboteqDriverState();
-
-  /**
-   * @brief Sends commands to the motors
-   *
-   * @param cmd command value in the range [-1000, 1000]
-   *
-   * @exception std::runtime_error if operation fails
-   */
-  void SendRoboteqCmd(const std::int32_t cmd_channel_1, const std::int32_t cmd_channel_2);
+  DriverState ReadDriverState() override;
 
   /**
    * @exception std::runtime_error if any operation returns error
    */
-  void ResetRoboteqScript();
+  void ResetScript() override;
 
   /**
    * @exception std::runtime_error if any operation returns error
    */
-  void TurnOnEStop();
+  void TurnOnEStop() override;
 
   /**
    * @exception std::runtime_error if any operation returns error
    */
-  void TurnOffEStop();
+  void TurnOffEStop() override;
 
   /**
-   * @brief Sends a safety stop command to the motor connected to channel 1
-   *
-   * @exception std::runtime_error if any operation returns error
+   * @brief Adds a motor driver to the driver
    */
-  void TurnOnSafetyStopChannel1();
+  void AddMotorDriver(const std::string name, std::shared_ptr<MotorDriver> motor_driver) override;
 
   /**
-   * @brief Sends a safety stop command to the motor connected to channel 2
+   * @brief Returns a motor driver by name
    *
-   * @exception std::runtime_error if any operation returns error
+   * @exception std::runtime_error if motor driver with the given name does not exist
    */
-  void TurnOnSafetyStopChannel2();
+  std::shared_ptr<MotorDriver> GetMotorDriver(const std::string & name) override;
 
-private:
   /**
    * @brief Blocking SDO write operation
    *
@@ -150,6 +103,28 @@ class RoboteqDriver : public lely::canopen::LoopDriver
   template <typename T>
   void SyncSDOWrite(const std::uint16_t index, const std::uint8_t subindex, const T data);
 
+  /**
+   * @brief Returns the last timestamp of the position data for the given channel
+   */
+  timespec GetPositionTimestamp(const std::uint8_t channel)
+  {
+    std::lock_guard<std::mutex> lck(position_timestamp_mtx_);
+    return last_position_timestamps_.at(channel);
+  }
+
+  /**
+   * @brief Returns the last timestamp of the speed and current data for the given channel
+   */
+  timespec GetSpeedCurrentTimestamp(const std::uint8_t channel)
+  {
+    std::lock_guard<std::mutex> lck(speed_current_timestamp_mtx_);
+    return last_speed_current_timestamps_.at(channel);
+  }
+
+  static constexpr std::uint8_t kChannel1 = 1;
+  static constexpr std::uint8_t kChannel2 = 2;
+
+private:
   void OnBoot(
     const lely::canopen::NmtState st, const char es, const std::string & what) noexcept override;
   void OnRpdoWrite(const std::uint16_t idx, const std::uint8_t subidx) noexcept override;
@@ -167,10 +142,11 @@ class RoboteqDriver : public lely::canopen::LoopDriver
   std::atomic_bool heartbeat_timeout_;
 
   std::mutex position_timestamp_mtx_;
-  timespec last_position_timestamp_;
+  std::map<std::uint8_t, timespec> last_position_timestamps_ = {{kChannel1, {}}, {kChannel2, {}}};
 
   std::mutex speed_current_timestamp_mtx_;
-  timespec last_speed_current_timestamp_;
+  std::map<std::uint8_t, timespec> last_speed_current_timestamps_ = {
+    {kChannel1, {}}, {kChannel2, {}}};
 
   std::mutex flags_current_timestamp_mtx_;
   timespec flags_current_timestamp_;
@@ -179,6 +155,43 @@ class RoboteqDriver : public lely::canopen::LoopDriver
   timespec last_voltages_temps_timestamp_;
 
   const std::chrono::milliseconds sdo_operation_timeout_ms_;
+
+  std::map<std::string, std::shared_ptr<MotorDriver>> motor_drivers_;
+};
+
+class RoboteqMotorDriver : public MotorDriver
+{
+public:
+  RoboteqMotorDriver(std::shared_ptr<RoboteqDriver> driver, const std::uint8_t channel)
+  : driver_(driver), channel_(channel)
+  {
+  }
+
+  /**
+   * @brief Reads motor state data and saves last timestamps
+   */
+  MotorDriverState ReadMotorDriverState() override;
+
+  /**
+   * @brief Sends commands to the motors
+   *
+   * @param cmd command value in the range [-1000, 1000]
+   *
+   * @exception std::runtime_error if operation fails
+   */
+  void SendCmdVel(const std::int32_t cmd) override;
+
+  /**
+   * @brief Sends a safety stop command to the motor connected to channel 1
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  void TurnOnSafetyStop() override;
+
+private:
+  std::weak_ptr<RoboteqDriver> driver_;
+
+  const std::uint8_t channel_;
 };
 
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/package.xml b/panther_hardware_interfaces/package.xml
index f3a90b58..df075d21 100644
--- a/panther_hardware_interfaces/package.xml
+++ b/panther_hardware_interfaces/package.xml
@@ -11,10 +11,11 @@
   <url type="repository">https://github.com/husarion/panther_ros</url>
   <url type="bugtracker">https://github.com/husarion/panther_ros/issues</url>
 
-  <author email="maciej.stepien@husarion.com">Maciej Stępień</author>
-  <author email="pawel.kowalski@husarion.com">Paweł Kowalski</author>
   <author email="jakub.delicat@husarion.com">Jakub Delicat</author>
   <author email="pawel.irzyk@husarion.com">Paweł Irzyk</author>
+  <author email="dawid.kmak@husarion.com">Dawid Kmak</author>
+  <author email="pawel.kowalski@husarion.com">Paweł Kowalski</author>
+  <author email="maciej.stepien@husarion.com">Maciej Stępień</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp b/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_manager.cpp
similarity index 63%
rename from panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp
rename to panther_hardware_interfaces/src/panther_system/motors_controller/canopen_manager.cpp
index 18f4117c..cf689bc7 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp
+++ b/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_manager.cpp
@@ -12,11 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp"
+#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp"
 
 #include <condition_variable>
 #include <filesystem>
-#include <future>
 #include <iostream>
 #include <memory>
 #include <mutex>
@@ -30,12 +29,12 @@
 namespace panther_hardware_interfaces
 {
 
-CANopenController::CANopenController(const CANopenSettings & canopen_settings)
+CANopenManager::CANopenManager(const CANopenSettings & canopen_settings)
 : canopen_settings_(canopen_settings)
 {
 }
 
-void CANopenController::Initialize()
+void CANopenManager::Initialize()
 {
   if (initialized_) {
     return;
@@ -43,24 +42,33 @@ void CANopenController::Initialize()
 
   canopen_communication_started_.store(false);
 
-  canopen_communication_thread_ = std::thread([this]() {
-    try {
-      panther_utils::ConfigureRT(kCANopenThreadSchedPriority);
-    } catch (const std::runtime_error & e) {
-      std::cerr << "An exception occurred while configuring RT: " << e.what() << std::endl
-                << "Continuing with regular thread settings (it may have a negative impact on the "
-                   "performance)."
-                << std::endl;
-    }
+  try {
+    panther_utils::ConfigureRT(kCANopenThreadSchedPriority);
+  } catch (const std::runtime_error & e) {
+    std::cerr << "An exception occurred while configuring RT: " << e.what() << std::endl
+              << "Continuing with regular thread settings (it may have a negative impact on the "
+                 "performance)."
+              << std::endl;
+  }
 
-    try {
-      InitializeCANCommunication();
-    } catch (const std::system_error & e) {
-      std::cerr << "An exception occurred while initializing CAN: " << e.what() << std::endl;
-      NotifyCANCommunicationStarted(false);
-      return;
-    }
+  try {
+    InitializeCANCommunication();
+  } catch (const std::system_error & e) {
+    std::cerr << "An exception occurred while initializing CAN: " << e.what() << std::endl;
+    NotifyCANCommunicationStarted(false);
+    return;
+  }
 
+  initialized_ = true;
+}
+
+void CANopenManager::Activate()
+{
+  if (!initialized_) {
+    throw std::runtime_error("CANopenManager not initialized.");
+  }
+
+  canopen_communication_thread_ = std::thread([this]() {
     NotifyCANCommunicationStarted(true);
 
     try {
@@ -78,15 +86,11 @@ void CANopenController::Initialize()
   }
 
   if (!canopen_communication_started_.load()) {
-    throw std::runtime_error("CAN communication not initialized.");
+    throw std::runtime_error("CAN communication not activated.");
   }
-
-  BootDrivers();
-
-  initialized_ = true;
 }
 
-void CANopenController::Deinitialize()
+void CANopenManager::Deinitialize()
 {
   // Deinitialization should be done regardless of the initialized_ state - in case some operation
   // during initialization fails, it is still necessary to do the cleanup
@@ -101,8 +105,6 @@ void CANopenController::Deinitialize()
 
   canopen_communication_started_.store(false);
 
-  rear_driver_.reset();
-  front_driver_.reset();
   master_.reset();
   chan_.reset();
   ctrl_.reset();
@@ -115,7 +117,7 @@ void CANopenController::Deinitialize()
   initialized_ = false;
 }
 
-void CANopenController::InitializeCANCommunication()
+void CANopenManager::InitializeCANCommunication()
 {
   lely::io::IoGuard io_guard;
 
@@ -141,16 +143,11 @@ void CANopenController::InitializeCANCommunication()
   master_ = std::make_shared<lely::canopen::AsyncMaster>(
     *timer_, *chan_, master_dcf_path, "", canopen_settings_.master_can_id);
 
-  front_driver_ = std::make_shared<RoboteqDriver>(
-    master_, canopen_settings_.front_driver_can_id, canopen_settings_.sdo_operation_timeout_ms);
-  rear_driver_ = std::make_shared<RoboteqDriver>(
-    master_, canopen_settings_.rear_driver_can_id, canopen_settings_.sdo_operation_timeout_ms);
-
   // Start the NMT service of the master by pretending to receive a 'reset node' command.
   master_->Reset();
 }
 
-void CANopenController::NotifyCANCommunicationStarted(const bool result)
+void CANopenManager::NotifyCANCommunicationStarted(const bool result)
 {
   {
     std::lock_guard<std::mutex> lck_g(canopen_communication_started_mtx_);
@@ -159,32 +156,4 @@ void CANopenController::NotifyCANCommunicationStarted(const bool result)
   canopen_communication_started_cond_.notify_all();
 }
 
-void CANopenController::BootDrivers()
-{
-  try {
-    auto front_driver_future = front_driver_->Boot();
-    auto rear_driver_future = rear_driver_->Boot();
-
-    auto front_driver_status = front_driver_future.wait_for(std::chrono::seconds(5));
-    auto rear_driver_status = rear_driver_future.wait_for(std::chrono::seconds(5));
-
-    if (
-      front_driver_status == std::future_status::ready &&
-      rear_driver_status == std::future_status::ready) {
-      try {
-        front_driver_future.get();
-        rear_driver_future.get();
-      } catch (const std::exception & e) {
-        throw std::runtime_error("Boot failed with exception: " + std::string(e.what()));
-      }
-    } else {
-      throw std::runtime_error("Boot timed out or failed.");
-    }
-
-  } catch (const std::system_error & e) {
-    throw std::runtime_error(
-      "An exception occurred while trying to Boot driver " + std::string(e.what()));
-  }
-}
-
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_data_converters.cpp b/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_data_converters.cpp
index d8d6991a..4c877913 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_data_converters.cpp
+++ b/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_data_converters.cpp
@@ -203,7 +203,7 @@ std::map<std::string, bool> RuntimeError::GetErrorMap() const
 }
 
 void RoboteqData::SetMotorsStates(
-  const RoboteqMotorState & left_state, const RoboteqMotorState & right_state,
+  const MotorDriverState & left_state, const MotorDriverState & right_state,
   const bool data_timed_out)
 {
   left_motor_state_.SetData(left_state);
@@ -211,7 +211,7 @@ void RoboteqData::SetMotorsStates(
   motor_states_data_timed_out_ = data_timed_out;
 }
 
-void RoboteqData::SetDriverState(const RoboteqDriverState & state, const bool data_timed_out)
+void RoboteqData::SetDriverState(const DriverState & state, const bool data_timed_out)
 {
   driver_state_.SetTemperature(state.mcu_temp);
   driver_state_.SetHeatsinkTemperature(state.heatsink_temp);
diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp b/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp
index ec9bf523..93340174 100644
--- a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp
@@ -24,34 +24,22 @@
 #include <string>
 #include <system_error>
 
+#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp"
 #include "panther_hardware_interfaces/utils.hpp"
 
 namespace panther_hardware_interfaces
 {
 
-struct CANopenObject
-{
-  const std::uint16_t id;
-  const std::uint8_t subid;
-};
-
 // All ids and sub ids were read directly from the eds file. Lely CANopen doesn't have the option
 // to parse them based on the ParameterName. Additionally between version v60 and v80
 // ParameterName changed, for example: Cmd_ESTOP (old), Cmd_ESTOP Emergency Shutdown (new) As
 // parameter names changed, but ids stayed the same, it will be better to just use ids directly
 struct RoboteqCANObjects
 {
-  static constexpr CANopenObject cmd_1 = {0x2000, 1};
-  static constexpr CANopenObject cmd_2 = {0x2000, 2};
-
-  static constexpr CANopenObject position_1 = {0x2104, 1};
-  static constexpr CANopenObject position_2 = {0x2104, 2};
-
-  static constexpr CANopenObject velocity_1 = {0x2107, 1};
-  static constexpr CANopenObject velocity_2 = {0x2107, 2};
-
-  static constexpr CANopenObject current_1 = {0x2100, 1};
-  static constexpr CANopenObject current_2 = {0x2100, 2};
+  static constexpr std::uint16_t cmd_id = 0x2000;
+  static constexpr std::uint16_t position_id = 0x2104;
+  static constexpr std::uint16_t velocity_id = 0x2107;
+  static constexpr std::uint16_t current_id = 0x2100;
 
   static constexpr CANopenObject flags = {0x2106, 7};
 
@@ -67,6 +55,44 @@ struct RoboteqCANObjects
   static constexpr CANopenObject turn_on_safety_stop = {0x202C, 0};  // Cmd_SFT
 };
 
+MotorDriverState RoboteqMotorDriver::ReadMotorDriverState()
+{
+  MotorDriverState state;
+
+  if (auto driver = driver_.lock()) {
+    state.pos = driver->rpdo_mapped[RoboteqCANObjects::position_id][channel_];
+    state.vel = driver->rpdo_mapped[RoboteqCANObjects::velocity_id][channel_];
+    state.current = driver->rpdo_mapped[RoboteqCANObjects::current_id][channel_];
+    state.pos_timestamp = driver->GetPositionTimestamp(channel_);
+    state.vel_current_timestamp = driver->GetSpeedCurrentTimestamp(channel_);
+  }
+
+  return state;
+}
+
+void RoboteqMotorDriver::SendCmdVel(const std::int32_t cmd)
+{
+  if (auto driver = driver_.lock()) {
+    driver->tpdo_mapped[RoboteqCANObjects::cmd_id][channel_] = cmd;
+    driver->tpdo_mapped[RoboteqCANObjects::cmd_id][channel_].WriteEvent();
+  }
+}
+
+void RoboteqMotorDriver::TurnOnSafetyStop()
+{
+  try {
+    if (auto driver = driver_.lock()) {
+      driver->SyncSDOWrite<std::uint8_t>(
+        RoboteqCANObjects::turn_on_safety_stop.id, RoboteqCANObjects::turn_on_safety_stop.subid,
+        channel_);
+    }
+  } catch (const std::runtime_error & e) {
+    throw std::runtime_error(
+      "Error when trying to turn on safety stop on channel " + std::to_string(channel_) + ": " +
+      std::string(e.what()));
+  }
+}
+
 RoboteqDriver::RoboteqDriver(
   const std::shared_ptr<lely::canopen::AsyncMaster> & master, const std::uint8_t id,
   const std::chrono::milliseconds & sdo_operation_timeout_ms)
@@ -77,6 +103,7 @@ RoboteqDriver::RoboteqDriver(
 std::future<void> RoboteqDriver::Boot()
 {
   std::lock_guard<std::mutex> lck(boot_mtx_);
+  boot_promise_ = std::promise<void>();
   std::future<void> future = boot_promise_.get_future();
 
   if (!LoopDriver::Boot()) {
@@ -85,42 +112,9 @@ std::future<void> RoboteqDriver::Boot()
   return future;
 }
 
-RoboteqMotorsStates RoboteqDriver::ReadRoboteqMotorsStates()
+DriverState RoboteqDriver::ReadDriverState()
 {
-  RoboteqMotorsStates states;
-
-  // already does locking when accessing rpdo
-  states.motor_1.pos =
-    rpdo_mapped[RoboteqCANObjects::position_1.id][RoboteqCANObjects::position_1.subid];
-  states.motor_2.pos =
-    rpdo_mapped[RoboteqCANObjects::position_2.id][RoboteqCANObjects::position_2.subid];
-
-  states.motor_1.vel =
-    rpdo_mapped[RoboteqCANObjects::velocity_1.id][RoboteqCANObjects::velocity_1.subid];
-  states.motor_2.vel =
-    rpdo_mapped[RoboteqCANObjects::velocity_2.id][RoboteqCANObjects::velocity_2.subid];
-
-  states.motor_1.current =
-    rpdo_mapped[RoboteqCANObjects::current_1.id][RoboteqCANObjects::current_1.subid];
-  states.motor_2.current =
-    rpdo_mapped[RoboteqCANObjects::current_2.id][RoboteqCANObjects::current_2.subid];
-
-  {
-    std::lock_guard<std::mutex> lck_p(position_timestamp_mtx_);
-    states.pos_timestamp = last_position_timestamp_;
-  }
-
-  {
-    std::lock_guard<std::mutex> lck_sc(speed_current_timestamp_mtx_);
-    states.vel_current_timestamp = last_speed_current_timestamp_;
-  }
-
-  return states;
-}
-
-RoboteqDriverState RoboteqDriver::ReadRoboteqDriverState()
-{
-  RoboteqDriverState state;
+  DriverState state;
 
   std::int32_t flags = static_cast<std::int32_t>(
     rpdo_mapped[RoboteqCANObjects::flags.id][RoboteqCANObjects::flags.subid]);
@@ -152,18 +146,7 @@ RoboteqDriverState RoboteqDriver::ReadRoboteqDriverState()
   return state;
 }
 
-void RoboteqDriver::SendRoboteqCmd(
-  const std::int32_t cmd_channel_1, const std::int32_t cmd_channel_2)
-{
-  tpdo_mapped[RoboteqCANObjects::cmd_1.id][RoboteqCANObjects::cmd_1.subid] = cmd_channel_1;
-  tpdo_mapped[RoboteqCANObjects::cmd_2.id][RoboteqCANObjects::cmd_2.subid] = cmd_channel_2;
-
-  // Both commands are in the TPDO 1, so write event for only one subid is triggered, as it will
-  // result in sending the whole TPDO 1 (both commands)
-  tpdo_mapped[RoboteqCANObjects::cmd_2.id][RoboteqCANObjects::cmd_2.subid].WriteEvent();
-}
-
-void RoboteqDriver::ResetRoboteqScript()
+void RoboteqDriver::ResetScript()
 {
   try {
     SyncSDOWrite<std::uint8_t>(
@@ -193,26 +176,23 @@ void RoboteqDriver::TurnOffEStop()
   }
 }
 
-void RoboteqDriver::TurnOnSafetyStopChannel1()
+void RoboteqDriver::AddMotorDriver(
+  const std::string name, std::shared_ptr<MotorDriver> motor_driver)
 {
-  try {
-    SyncSDOWrite<std::uint8_t>(
-      RoboteqCANObjects::turn_on_safety_stop.id, RoboteqCANObjects::turn_on_safety_stop.subid, 1);
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Error when trying to turn on safety stop on channel 1: " + std::string(e.what()));
+  if (std::dynamic_pointer_cast<RoboteqMotorDriver>(motor_driver) == nullptr) {
+    throw std::runtime_error("Motor driver is not of type RoboteqMotorDriver");
   }
+  motor_drivers_.emplace(name, motor_driver);
 }
 
-void RoboteqDriver::TurnOnSafetyStopChannel2()
+std::shared_ptr<MotorDriver> RoboteqDriver::GetMotorDriver(const std::string & name)
 {
-  try {
-    SyncSDOWrite<std::uint8_t>(
-      RoboteqCANObjects::turn_on_safety_stop.id, RoboteqCANObjects::turn_on_safety_stop.subid, 2);
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Error when trying to turn on safety stop on channel 2: " + std::string(e.what()));
+  auto it = motor_drivers_.find(name);
+  if (it == motor_drivers_.end()) {
+    throw std::runtime_error("Motor driver with name '" + name + "' does not exist");
   }
+
+  return it->second;
 }
 
 template <typename T>
@@ -270,13 +250,18 @@ void RoboteqDriver::OnBoot(
 
 void RoboteqDriver::OnRpdoWrite(const std::uint16_t idx, const std::uint8_t subidx) noexcept
 {
-  if (idx == RoboteqCANObjects::position_1.id && subidx == RoboteqCANObjects::position_1.subid) {
+  if (idx == RoboteqCANObjects::position_id) {
+    if (subidx != kChannel1 && subidx != kChannel2) {
+      return;
+    }
     std::lock_guard<std::mutex> lck(position_timestamp_mtx_);
-    clock_gettime(CLOCK_MONOTONIC, &last_position_timestamp_);
-  } else if (
-    idx == RoboteqCANObjects::velocity_1.id && subidx == RoboteqCANObjects::velocity_1.subid) {
+    clock_gettime(CLOCK_MONOTONIC, &last_position_timestamps_.at(subidx));
+  } else if (idx == RoboteqCANObjects::velocity_id) {
+    if (subidx != kChannel1 && subidx != kChannel2) {
+      return;
+    }
     std::lock_guard<std::mutex> lck(speed_current_timestamp_mtx_);
-    clock_gettime(CLOCK_MONOTONIC, &last_speed_current_timestamp_);
+    clock_gettime(CLOCK_MONOTONIC, &last_speed_current_timestamps_.at(subidx));
   } else if (idx == RoboteqCANObjects::flags.id && subidx == RoboteqCANObjects::flags.subid) {
     std::lock_guard<std::mutex> lck(flags_current_timestamp_mtx_);
     clock_gettime(CLOCK_MONOTONIC, &flags_current_timestamp_);
diff --git a/panther_hardware_interfaces/test/config/canopen_configuration.yaml b/panther_hardware_interfaces/test/config/canopen_configuration.yaml
index 8f0a70a2..bef2f4cd 100644
--- a/panther_hardware_interfaces/test/config/canopen_configuration.yaml
+++ b/panther_hardware_interfaces/test/config/canopen_configuration.yaml
@@ -6,8 +6,3 @@ slave_1:
   dcf: roboteq_motor_controllers_v80_21a.eds
   node_id: 1
   heartbeat_producer: 100
-
-slave_2:
-  dcf: roboteq_motor_controllers_v80_21a.eds
-  node_id: 2
-  heartbeat_producer: 100
diff --git a/panther_hardware_interfaces/test/config/slave_2.bin b/panther_hardware_interfaces/test/config/slave_2.bin
deleted file mode 100644
index c042c65d87474cff1e582580cc27e9c59c20250e..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 13
ScmZQ%U|<jzU|<4LDGUGtJphjY

diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp b/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp
deleted file mode 100644
index dc1c9a5f..00000000
--- a/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp
+++ /dev/null
@@ -1,101 +0,0 @@
-// Copyright 2023 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 <chrono>
-#include <cstdint>
-#include <memory>
-#include <stdexcept>
-#include <string>
-
-#include <gtest/gtest.h>
-
-#include <panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp>
-
-#include "utils/roboteqs_mock.hpp"
-#include "utils/test_constants.hpp"
-
-class TestCANopenController : public ::testing::Test
-{
-public:
-  TestCANopenController()
-  {
-    canopen_controller_ = std::make_unique<panther_hardware_interfaces::CANopenController>(
-      panther_hardware_interfaces_test::kCANopenSettings);
-
-    roboteqs_mock_ = std::make_unique<panther_hardware_interfaces_test::RoboteqsMock>();
-    roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
-  }
-
-  ~TestCANopenController()
-  {
-    roboteqs_mock_->Stop();
-    roboteqs_mock_.reset();
-  }
-
-protected:
-  std::unique_ptr<panther_hardware_interfaces_test::RoboteqsMock> roboteqs_mock_;
-  std::unique_ptr<panther_hardware_interfaces::CANopenController> canopen_controller_;
-};
-
-TEST_F(TestCANopenController, InitializeAndDeinitialize)
-{
-  ASSERT_NO_THROW(canopen_controller_->Initialize());
-  ASSERT_NO_THROW(canopen_controller_->Deinitialize());
-
-  // Check if deinitialization worked correctly - initialize once again
-  ASSERT_NO_THROW(canopen_controller_->Initialize());
-  ASSERT_NO_THROW(canopen_controller_->Deinitialize());
-}
-
-TEST_F(TestCANopenController, InitializeErrorDeviceType)
-{
-  roboteqs_mock_->GetFrontDriver()->SetOnReadWait<std::uint32_t>(0x1000, 0, 100000);
-  ASSERT_THROW(canopen_controller_->Initialize(), std::runtime_error);
-  ASSERT_NO_THROW(canopen_controller_->Deinitialize());
-
-  roboteqs_mock_->GetFrontDriver()->SetOnReadWait<std::uint32_t>(0x1000, 0, 0);
-  ASSERT_NO_THROW(canopen_controller_->Initialize());
-  ASSERT_NO_THROW(canopen_controller_->Deinitialize());
-}
-
-TEST_F(TestCANopenController, InitializeErrorVendorId)
-{
-  roboteqs_mock_->GetRearDriver()->SetOnReadWait<std::uint32_t>(0x1018, 1, 100000);
-  ASSERT_THROW(canopen_controller_->Initialize(), std::runtime_error);
-  ASSERT_NO_THROW(canopen_controller_->Deinitialize());
-
-  roboteqs_mock_->GetRearDriver()->SetOnReadWait<std::uint32_t>(0x1018, 1, 0);
-  ASSERT_NO_THROW(canopen_controller_->Initialize());
-  ASSERT_NO_THROW(canopen_controller_->Deinitialize());
-}
-
-TEST(TestCANopenControllerOthers, BootTimeout)
-{
-  std::unique_ptr<panther_hardware_interfaces::CANopenController> canopen_controller_;
-
-  canopen_controller_ = std::make_unique<panther_hardware_interfaces::CANopenController>(
-    panther_hardware_interfaces_test::kCANopenSettings);
-
-  // No roboteq mock, so it won't be possible to boot - here is checked if after some time it will
-  // finish with exception
-  ASSERT_THROW(canopen_controller_->Initialize(), std::runtime_error);
-
-  canopen_controller_->Deinitialize();
-}
-
-int main(int argc, char ** argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}
diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_driver.cpp
deleted file mode 100644
index 7f83e88d..00000000
--- a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_driver.cpp
+++ /dev/null
@@ -1,390 +0,0 @@
-// Copyright 2023 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 <chrono>
-#include <cstdint>
-#include <memory>
-#include <string>
-#include <thread>
-
-#include <gtest/gtest.h>
-
-#include <panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp>
-#include <panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp>
-
-#include "utils/roboteqs_mock.hpp"
-#include "utils/test_constants.hpp"
-
-#include <iostream>
-
-class TestRoboteqDriver : public ::testing::Test
-{
-public:
-  TestRoboteqDriver()
-  {
-    canopen_controller_ = std::make_unique<panther_hardware_interfaces::CANopenController>(
-      panther_hardware_interfaces_test::kCANopenSettings);
-
-    roboteqs_mock_ = std::make_unique<panther_hardware_interfaces_test::RoboteqsMock>();
-    roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
-    canopen_controller_->Initialize();
-  }
-
-  ~TestRoboteqDriver()
-  {
-    canopen_controller_->Deinitialize();
-    roboteqs_mock_->Stop();
-    roboteqs_mock_.reset();
-  }
-
-protected:
-  std::unique_ptr<panther_hardware_interfaces_test::RoboteqsMock> roboteqs_mock_;
-  std::unique_ptr<panther_hardware_interfaces::CANopenController> canopen_controller_;
-};
-
-TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
-{
-  using panther_hardware_interfaces_test::DriverChannel;
-
-  const std::int32_t fl_pos = 101;
-  const std::int32_t fl_vel = 102;
-  const std::int32_t fl_current = 103;
-  const std::int32_t fr_pos = 201;
-  const std::int32_t fr_vel = 202;
-  const std::int32_t fr_current = 203;
-  const std::int32_t rl_pos = 301;
-  const std::int32_t rl_vel = 302;
-  const std::int32_t rl_current = 303;
-  const std::int32_t rr_pos = 401;
-  const std::int32_t rr_vel = 402;
-  const std::int32_t rr_current = 403;
-
-  roboteqs_mock_->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL2, fl_pos);
-  roboteqs_mock_->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL1, fr_pos);
-  roboteqs_mock_->GetRearDriver()->SetPosition(DriverChannel::CHANNEL2, rl_pos);
-  roboteqs_mock_->GetRearDriver()->SetPosition(DriverChannel::CHANNEL1, rr_pos);
-
-  roboteqs_mock_->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL2, fl_vel);
-  roboteqs_mock_->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL1, fr_vel);
-  roboteqs_mock_->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL2, rl_vel);
-  roboteqs_mock_->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL1, rr_vel);
-
-  roboteqs_mock_->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL2, fl_current);
-  roboteqs_mock_->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL1, fr_current);
-  roboteqs_mock_->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL2, rl_current);
-  roboteqs_mock_->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL1, rr_current);
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-
-  panther_hardware_interfaces::RoboteqMotorsStates f_fb =
-    canopen_controller_->GetFrontDriver()->ReadRoboteqMotorsStates();
-  panther_hardware_interfaces::RoboteqMotorsStates r_fb =
-    canopen_controller_->GetRearDriver()->ReadRoboteqMotorsStates();
-
-  EXPECT_EQ(f_fb.motor_2.pos, fl_pos);
-  EXPECT_EQ(f_fb.motor_2.vel, fl_vel);
-  EXPECT_EQ(f_fb.motor_2.current, fl_current);
-
-  EXPECT_EQ(f_fb.motor_1.pos, fr_pos);
-  EXPECT_EQ(f_fb.motor_1.vel, fr_vel);
-  EXPECT_EQ(f_fb.motor_1.current, fr_current);
-
-  EXPECT_EQ(r_fb.motor_2.pos, rl_pos);
-  EXPECT_EQ(r_fb.motor_2.vel, rl_vel);
-  EXPECT_EQ(r_fb.motor_2.current, rl_current);
-
-  EXPECT_EQ(r_fb.motor_1.pos, rr_pos);
-  EXPECT_EQ(r_fb.motor_1.vel, rr_vel);
-  EXPECT_EQ(r_fb.motor_1.current, rr_current);
-}
-
-TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps)
-{
-  std::this_thread::sleep_for(std::chrono::milliseconds(150));
-
-  panther_hardware_interfaces::RoboteqMotorsStates f_fb1 =
-    canopen_controller_->GetFrontDriver()->ReadRoboteqMotorsStates();
-  panther_hardware_interfaces::RoboteqMotorsStates r_fb1 =
-    canopen_controller_->GetRearDriver()->ReadRoboteqMotorsStates();
-
-  // based on publishing frequency in the Roboteq mock (10)
-  std::this_thread::sleep_for(std::chrono::milliseconds(10));
-
-  panther_hardware_interfaces::RoboteqMotorsStates f_fb2 =
-    canopen_controller_->GetFrontDriver()->ReadRoboteqMotorsStates();
-  panther_hardware_interfaces::RoboteqMotorsStates r_fb2 =
-    canopen_controller_->GetRearDriver()->ReadRoboteqMotorsStates();
-
-  // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
-  // if consecutive messages will have timestamps 100ms + some threshold apart
-  EXPECT_LE(
-    lely::util::from_timespec(f_fb2.pos_timestamp) - lely::util::from_timespec(f_fb1.pos_timestamp),
-    std::chrono::milliseconds(15));
-  EXPECT_LE(
-    lely::util::from_timespec(r_fb2.pos_timestamp) - lely::util::from_timespec(r_fb1.pos_timestamp),
-    std::chrono::milliseconds(15));
-
-  EXPECT_GE(
-    lely::util::from_timespec(f_fb2.pos_timestamp) - lely::util::from_timespec(f_fb1.pos_timestamp),
-    std::chrono::milliseconds(5));
-  EXPECT_GE(
-    lely::util::from_timespec(r_fb2.pos_timestamp) - lely::util::from_timespec(r_fb1.pos_timestamp),
-    std::chrono::milliseconds(5));
-
-  EXPECT_LE(
-    lely::util::from_timespec(f_fb2.vel_current_timestamp) -
-      lely::util::from_timespec(f_fb1.vel_current_timestamp),
-    std::chrono::milliseconds(15));
-  EXPECT_LE(
-    lely::util::from_timespec(r_fb2.vel_current_timestamp) -
-      lely::util::from_timespec(r_fb1.vel_current_timestamp),
-    std::chrono::milliseconds(15));
-
-  EXPECT_GE(
-    lely::util::from_timespec(f_fb2.vel_current_timestamp) -
-      lely::util::from_timespec(f_fb1.vel_current_timestamp),
-    std::chrono::milliseconds(5));
-  EXPECT_GE(
-    lely::util::from_timespec(r_fb2.vel_current_timestamp) -
-      lely::util::from_timespec(r_fb1.vel_current_timestamp),
-    std::chrono::milliseconds(5));
-}
-
-TEST_F(TestRoboteqDriver, ReadRoboteqDriverState)
-{
-  using panther_hardware_interfaces_test::DriverChannel;
-  using panther_hardware_interfaces_test::DriverFaultFlags;
-  using panther_hardware_interfaces_test::DriverRuntimeErrors;
-  using panther_hardware_interfaces_test::DriverScriptFlags;
-
-  const std::int16_t f_temp = 30;
-  const std::int16_t r_temp = 32;
-  const std::int16_t f_heatsink_temp = 31;
-  const std::int16_t r_heatsink_temp = 33;
-  const std::uint16_t f_volt = 400;
-  const std::uint16_t r_volt = 430;
-  const std::int16_t f_battery_current_1 = 10;
-  const std::int16_t r_battery_current_1 = 30;
-  const std::int16_t f_battery_current_2 = 30;
-  const std::int16_t r_battery_current_2 = 40;
-
-  roboteqs_mock_->GetFrontDriver()->SetTemperature(f_temp);
-  roboteqs_mock_->GetRearDriver()->SetTemperature(r_temp);
-  roboteqs_mock_->GetFrontDriver()->SetHeatsinkTemperature(f_heatsink_temp);
-  roboteqs_mock_->GetRearDriver()->SetHeatsinkTemperature(r_heatsink_temp);
-  roboteqs_mock_->GetFrontDriver()->SetVoltage(f_volt);
-  roboteqs_mock_->GetRearDriver()->SetVoltage(r_volt);
-  roboteqs_mock_->GetFrontDriver()->SetBatteryCurrent1(f_battery_current_1);
-  roboteqs_mock_->GetRearDriver()->SetBatteryCurrent1(r_battery_current_1);
-  roboteqs_mock_->GetFrontDriver()->SetBatteryCurrent2(f_battery_current_2);
-  roboteqs_mock_->GetRearDriver()->SetBatteryCurrent2(r_battery_current_2);
-
-  roboteqs_mock_->GetFrontDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERHEAT);
-  roboteqs_mock_->GetFrontDriver()->SetDriverScriptFlag(DriverScriptFlags::ENCODER_DISCONNECTED);
-  roboteqs_mock_->GetFrontDriver()->SetDriverRuntimeError(
-    DriverChannel::CHANNEL1, DriverRuntimeErrors::LOOP_ERROR);
-  roboteqs_mock_->GetFrontDriver()->SetDriverRuntimeError(
-    DriverChannel::CHANNEL2, DriverRuntimeErrors::SAFETY_STOP_ACTIVE);
-
-  roboteqs_mock_->GetRearDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERVOLTAGE);
-  roboteqs_mock_->GetRearDriver()->SetDriverScriptFlag(DriverScriptFlags::AMP_LIMITER);
-  roboteqs_mock_->GetRearDriver()->SetDriverRuntimeError(
-    DriverChannel::CHANNEL1, DriverRuntimeErrors::FORWARD_LIMIT_TRIGGERED);
-  roboteqs_mock_->GetRearDriver()->SetDriverRuntimeError(
-    DriverChannel::CHANNEL2, DriverRuntimeErrors::REVERSE_LIMIT_TRIGGERED);
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-
-  panther_hardware_interfaces::RoboteqDriverState f_fb =
-    canopen_controller_->GetFrontDriver()->ReadRoboteqDriverState();
-  panther_hardware_interfaces::RoboteqDriverState r_fb =
-    canopen_controller_->GetRearDriver()->ReadRoboteqDriverState();
-
-  EXPECT_EQ(f_fb.mcu_temp, f_temp);
-  EXPECT_EQ(r_fb.mcu_temp, r_temp);
-
-  EXPECT_EQ(f_fb.heatsink_temp, f_heatsink_temp);
-  EXPECT_EQ(r_fb.heatsink_temp, r_heatsink_temp);
-
-  EXPECT_EQ(f_fb.battery_voltage, f_volt);
-  EXPECT_EQ(r_fb.battery_voltage, r_volt);
-
-  EXPECT_EQ(f_fb.battery_current_1, f_battery_current_1);
-  EXPECT_EQ(r_fb.battery_current_1, r_battery_current_1);
-
-  EXPECT_EQ(f_fb.battery_current_2, f_battery_current_2);
-  EXPECT_EQ(r_fb.battery_current_2, r_battery_current_2);
-
-  EXPECT_EQ(f_fb.fault_flags, 0b00000001);
-  EXPECT_EQ(f_fb.script_flags, 0b00000010);
-  EXPECT_EQ(f_fb.runtime_stat_flag_motor_1, 0b00000100);
-  EXPECT_EQ(f_fb.runtime_stat_flag_motor_2, 0b00001000);
-
-  EXPECT_EQ(r_fb.fault_flags, 0b00000010);
-  EXPECT_EQ(r_fb.script_flags, 0b00000100);
-  EXPECT_EQ(r_fb.runtime_stat_flag_motor_1, 0b00010000);
-  EXPECT_EQ(r_fb.runtime_stat_flag_motor_2, 0b00100000);
-}
-
-TEST_F(TestRoboteqDriver, ReadRoboteqDriverStateTimestamp)
-{
-  std::this_thread::sleep_for(std::chrono::milliseconds(150));
-
-  panther_hardware_interfaces::RoboteqDriverState f_fb1 =
-    canopen_controller_->GetFrontDriver()->ReadRoboteqDriverState();
-  panther_hardware_interfaces::RoboteqDriverState r_fb1 =
-    canopen_controller_->GetRearDriver()->ReadRoboteqDriverState();
-
-  // based on publishing frequency in the Roboteq mock (50)
-  std::this_thread::sleep_for(std::chrono::milliseconds(50));
-
-  panther_hardware_interfaces::RoboteqDriverState f_fb2 =
-    canopen_controller_->GetFrontDriver()->ReadRoboteqDriverState();
-  panther_hardware_interfaces::RoboteqDriverState r_fb2 =
-    canopen_controller_->GetRearDriver()->ReadRoboteqDriverState();
-
-  // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
-  // if consecutive messages will have timestamps 100ms + some threshold apart
-  EXPECT_LE(
-    lely::util::from_timespec(f_fb2.flags_current_timestamp) -
-      lely::util::from_timespec(f_fb1.flags_current_timestamp),
-    std::chrono::milliseconds(75));
-  EXPECT_LE(
-    lely::util::from_timespec(r_fb2.flags_current_timestamp) -
-      lely::util::from_timespec(r_fb1.flags_current_timestamp),
-    std::chrono::milliseconds(75));
-
-  EXPECT_GE(
-    lely::util::from_timespec(f_fb2.flags_current_timestamp) -
-      lely::util::from_timespec(f_fb1.flags_current_timestamp),
-    std::chrono::milliseconds(25));
-  EXPECT_GE(
-    lely::util::from_timespec(r_fb2.flags_current_timestamp) -
-      lely::util::from_timespec(r_fb1.flags_current_timestamp),
-    std::chrono::milliseconds(25));
-
-  EXPECT_LE(
-    lely::util::from_timespec(f_fb2.voltages_temps_timestamp) -
-      lely::util::from_timespec(f_fb1.voltages_temps_timestamp),
-    std::chrono::milliseconds(75));
-  EXPECT_LE(
-    lely::util::from_timespec(r_fb2.voltages_temps_timestamp) -
-      lely::util::from_timespec(r_fb1.voltages_temps_timestamp),
-    std::chrono::milliseconds(75));
-
-  EXPECT_GE(
-    lely::util::from_timespec(f_fb2.voltages_temps_timestamp) -
-      lely::util::from_timespec(f_fb1.voltages_temps_timestamp),
-    std::chrono::milliseconds(25));
-  EXPECT_GE(
-    lely::util::from_timespec(r_fb2.voltages_temps_timestamp) -
-      lely::util::from_timespec(r_fb1.voltages_temps_timestamp),
-    std::chrono::milliseconds(25));
-}
-
-TEST_F(TestRoboteqDriver, SendRoboteqCmd)
-{
-  using panther_hardware_interfaces_test::DriverChannel;
-
-  const std::int32_t fl_v = 10;
-  const std::int32_t fr_v = 20;
-  const std::int32_t rl_v = 30;
-  const std::int32_t rr_v = 40;
-
-  canopen_controller_->GetFrontDriver()->SendRoboteqCmd(fr_v, fl_v);
-  canopen_controller_->GetRearDriver()->SendRoboteqCmd(rr_v, rl_v);
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), fl_v);
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), fr_v);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), rl_v);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), rr_v);
-}
-
-TEST_F(TestRoboteqDriver, ResetRoboteqScript)
-{
-  roboteqs_mock_->GetFrontDriver()->SetResetRoboteqScript(65);
-  roboteqs_mock_->GetRearDriver()->SetResetRoboteqScript(23);
-
-  canopen_controller_->GetFrontDriver()->ResetRoboteqScript();
-  canopen_controller_->GetRearDriver()->ResetRoboteqScript();
-
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetResetRoboteqScript(), 2);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetResetRoboteqScript(), 2);
-}
-
-TEST_F(TestRoboteqDriver, ReadRoboteqTurnOnEStop)
-{
-  roboteqs_mock_->GetFrontDriver()->SetTurnOnEStop(65);
-  roboteqs_mock_->GetRearDriver()->SetTurnOnEStop(23);
-
-  canopen_controller_->GetFrontDriver()->TurnOnEStop();
-  canopen_controller_->GetRearDriver()->TurnOnEStop();
-
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOnEStop(), 1);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOnEStop(), 1);
-}
-
-TEST_F(TestRoboteqDriver, TurnOffEStop)
-{
-  roboteqs_mock_->GetFrontDriver()->SetTurnOffEStop(65);
-  roboteqs_mock_->GetRearDriver()->SetTurnOffEStop(23);
-
-  canopen_controller_->GetFrontDriver()->TurnOffEStop();
-  canopen_controller_->GetRearDriver()->TurnOffEStop();
-
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOffEStop(), 1);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOffEStop(), 1);
-}
-
-TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel1)
-{
-  roboteqs_mock_->GetFrontDriver()->SetTurnOnSafetyStop(67);
-  roboteqs_mock_->GetRearDriver()->SetTurnOnSafetyStop(21);
-
-  canopen_controller_->GetFrontDriver()->TurnOnSafetyStopChannel1();
-  canopen_controller_->GetRearDriver()->TurnOnSafetyStopChannel1();
-
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOnSafetyStop(), 1);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOnSafetyStop(), 1);
-}
-
-TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel2)
-{
-  roboteqs_mock_->GetFrontDriver()->SetTurnOnSafetyStop(65);
-  roboteqs_mock_->GetRearDriver()->SetTurnOnSafetyStop(23);
-
-  canopen_controller_->GetFrontDriver()->TurnOnSafetyStopChannel2();
-  canopen_controller_->GetRearDriver()->TurnOnSafetyStopChannel2();
-
-  EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOnSafetyStop(), 2);
-  EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOnSafetyStop(), 2);
-}
-
-TEST_F(TestRoboteqDriver, WriteTimeout)
-{
-  roboteqs_mock_->GetFrontDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 200000);
-  ASSERT_THROW(
-    canopen_controller_->GetFrontDriver()->TurnOnSafetyStopChannel1(), std::runtime_error);
-}
-
-// OnCanError isn't tested, because it reacts to lower-level CAN errors (CRC), which are hard to
-// simulate, but it would be nice to add it
-
-int main(int argc, char ** argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}
diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp
new file mode 100644
index 00000000..0291b5c8
--- /dev/null
+++ b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp
@@ -0,0 +1,101 @@
+// Copyright 2023 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 <memory>
+#include <stdexcept>
+#include <string>
+
+#include <gtest/gtest.h>
+#include <sys/socket.h>
+
+#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp"
+
+#include "utils/fake_can_socket.hpp"
+#include "utils/test_constants.hpp"
+
+#include "panther_utils/test/test_utils.hpp"
+
+class TestCANopenManager : public ::testing::Test
+{
+public:
+  TestCANopenManager();
+
+  ~TestCANopenManager() {}
+
+protected:
+  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
+  std::unique_ptr<panther_hardware_interfaces::CANopenManager> canopen_manager_;
+};
+
+TestCANopenManager::TestCANopenManager()
+{
+  can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
+    panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
+
+  canopen_manager_ = std::make_unique<panther_hardware_interfaces::CANopenManager>(
+    panther_hardware_interfaces_test::kCANopenSettings);
+}
+
+TEST_F(TestCANopenManager, InitializeAndDeinitialize)
+{
+  ASSERT_NO_THROW(canopen_manager_->Initialize());
+  ASSERT_NO_THROW(canopen_manager_->Deinitialize());
+
+  // Check if deinitialization worked correctly - initialize once again
+  ASSERT_NO_THROW(canopen_manager_->Initialize());
+}
+
+TEST_F(TestCANopenManager, Activate)
+{
+  can_socket_->Initialize();
+
+  ASSERT_NO_THROW(canopen_manager_->Initialize());
+  EXPECT_NO_THROW(canopen_manager_->Activate());
+  ASSERT_NO_THROW(canopen_manager_->Deinitialize());
+
+  // Check if deinitialization worked correctly - activate once again
+  ASSERT_NO_THROW(canopen_manager_->Initialize());
+  EXPECT_NO_THROW(canopen_manager_->Activate());
+}
+
+TEST_F(TestCANopenManager, ActivateNotInitialized)
+{
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { canopen_manager_->Activate(); }, "CANopenManager not initialized."));
+}
+
+TEST_F(TestCANopenManager, ActivateNoCommunication)
+{
+  ASSERT_NO_THROW(canopen_manager_->Initialize());
+  EXPECT_THROW(canopen_manager_->Activate(), std::runtime_error);
+}
+
+TEST_F(TestCANopenManager, GetMaster)
+{
+  can_socket_->Initialize();
+  ASSERT_NO_THROW(canopen_manager_->Initialize());
+  EXPECT_NO_THROW(canopen_manager_->GetMaster());
+}
+
+TEST_F(TestCANopenManager, GetMasterNotInitialized)
+{
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { canopen_manager_->GetMaster(); }, "CANopenManager not initialized."));
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
new file mode 100644
index 00000000..8b02967a
--- /dev/null
+++ b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
@@ -0,0 +1,390 @@
+// Copyright 2023 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 <chrono>
+#include <cstdint>
+#include <memory>
+#include <string>
+#include <thread>
+
+#include <gtest/gtest.h>
+
+#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp"
+
+#include "utils/fake_can_socket.hpp"
+#include "utils/mock_roboteq.hpp"
+#include "utils/test_constants.hpp"
+
+#include "panther_utils/test/test_utils.hpp"
+
+class TestRoboteqDriver : public ::testing::Test
+{
+public:
+  TestRoboteqDriver();
+
+  ~TestRoboteqDriver();
+
+  void BootRoboteqDriver();
+
+protected:
+  bool TestBoot();
+
+  static constexpr char kMotor1Name[] = "motor_1";
+  static constexpr char kMotor2Name[] = "motor_2";
+
+  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
+  std::unique_ptr<panther_hardware_interfaces_test::MockRoboteq> mock_roboteq_;
+  std::unique_ptr<panther_hardware_interfaces::CANopenManager> canopen_manager_;
+  std::shared_ptr<panther_hardware_interfaces::RoboteqDriver> roboteq_driver_;
+};
+
+TestRoboteqDriver::TestRoboteqDriver()
+{
+  can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
+    panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
+  can_socket_->Initialize();
+
+  canopen_manager_ = std::make_unique<panther_hardware_interfaces::CANopenManager>(
+    panther_hardware_interfaces_test::kCANopenSettings);
+
+  mock_roboteq_ = std::make_unique<panther_hardware_interfaces_test::MockRoboteq>();
+  mock_roboteq_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
+  canopen_manager_->Initialize();
+
+  roboteq_driver_ = std::make_shared<panther_hardware_interfaces::RoboteqDriver>(
+    canopen_manager_->GetMaster(), 1, std::chrono::milliseconds(100));
+
+  canopen_manager_->Activate();
+}
+
+TestRoboteqDriver::~TestRoboteqDriver()
+{
+  roboteq_driver_.reset();
+  canopen_manager_->Deinitialize();
+  mock_roboteq_->Stop();
+  mock_roboteq_.reset();
+  can_socket_->Deinitialize();
+}
+
+void TestRoboteqDriver::BootRoboteqDriver()
+{
+  auto motor_1 = std::make_shared<panther_hardware_interfaces::RoboteqMotorDriver>(
+    roboteq_driver_, panther_hardware_interfaces::RoboteqDriver::kChannel1);
+  auto motor_2 = std::make_shared<panther_hardware_interfaces::RoboteqMotorDriver>(
+    roboteq_driver_, panther_hardware_interfaces::RoboteqDriver::kChannel2);
+
+  roboteq_driver_->AddMotorDriver(kMotor1Name, motor_1);
+  roboteq_driver_->AddMotorDriver(kMotor2Name, motor_2);
+  auto future = roboteq_driver_->Boot();
+  future.wait();
+}
+
+bool TestRoboteqDriver::TestBoot()
+{
+  auto future = roboteq_driver_->Boot();
+  const auto future_status = future.wait_for(std::chrono::milliseconds(500));
+
+  if (future_status != std::future_status::ready) {
+    return false;
+  }
+
+  try {
+    future.get();
+  } catch (const std::exception & e) {
+    return false;
+  }
+
+  return true;
+}
+
+TEST_F(TestRoboteqDriver, BootRoboteqDriver) { ASSERT_TRUE(TestBoot()); }
+
+TEST_F(TestRoboteqDriver, BootErrorDeviceType)
+{
+  const auto device_type_id = 0x1000;
+  const auto device_type_subid = 0;
+  mock_roboteq_->GetDriver()->SetOnReadWait<std::uint32_t>(
+    device_type_id, device_type_subid, 100000);
+  EXPECT_FALSE(TestBoot());
+}
+
+TEST_F(TestRoboteqDriver, BootErrorVendorId)
+{
+  const auto vendor_id_id = 0x1018;
+  const auto vendor_id_subid = 1;
+  mock_roboteq_->GetDriver()->SetOnReadWait<std::uint32_t>(vendor_id_id, vendor_id_subid, 100000);
+  EXPECT_FALSE(TestBoot());
+}
+
+TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
+{
+  using panther_hardware_interfaces_test::DriverChannel;
+
+  const std::int32_t motor_1_pos = 101;
+  const std::int32_t motor_1_vel = 102;
+  const std::int32_t motor_1_current = 103;
+  const std::int32_t motor_2_pos = 201;
+  const std::int32_t motor_2_vel = 202;
+  const std::int32_t motor_2_current = 203;
+
+  BootRoboteqDriver();
+
+  mock_roboteq_->GetDriver()->SetPosition(DriverChannel::CHANNEL1, motor_1_pos);
+  mock_roboteq_->GetDriver()->SetPosition(DriverChannel::CHANNEL2, motor_2_pos);
+
+  mock_roboteq_->GetDriver()->SetVelocity(DriverChannel::CHANNEL1, motor_1_vel);
+  mock_roboteq_->GetDriver()->SetVelocity(DriverChannel::CHANNEL2, motor_2_vel);
+
+  mock_roboteq_->GetDriver()->SetCurrent(DriverChannel::CHANNEL1, motor_1_current);
+  mock_roboteq_->GetDriver()->SetCurrent(DriverChannel::CHANNEL2, motor_2_current);
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(10));
+
+  panther_hardware_interfaces::MotorDriverState motor_driver_state_1 =
+    roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadMotorDriverState();
+  panther_hardware_interfaces::MotorDriverState motor_driver_state_2 =
+    roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadMotorDriverState();
+
+  EXPECT_EQ(motor_driver_state_1.pos, motor_1_pos);
+  EXPECT_EQ(motor_driver_state_1.vel, motor_1_vel);
+  EXPECT_EQ(motor_driver_state_1.current, motor_1_current);
+
+  EXPECT_EQ(motor_driver_state_2.pos, motor_2_pos);
+  EXPECT_EQ(motor_driver_state_2.vel, motor_2_vel);
+  EXPECT_EQ(motor_driver_state_2.current, motor_2_current);
+}
+
+TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps)
+{
+  BootRoboteqDriver();
+
+  panther_hardware_interfaces::MotorDriverState motor_driver_state_1 =
+    roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadMotorDriverState();
+
+  // based on publishing frequency in the Roboteq mock (100Hz)
+  std::this_thread::sleep_for(std::chrono::milliseconds(10));
+
+  panther_hardware_interfaces::MotorDriverState motor_driver_state_2 =
+    roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadMotorDriverState();
+
+  // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
+  // if consecutive messages will have timestamps 100ms + some threshold apart
+  EXPECT_LE(
+    lely::util::from_timespec(motor_driver_state_2.pos_timestamp) -
+      lely::util::from_timespec(motor_driver_state_1.pos_timestamp),
+    std::chrono::milliseconds(15));
+
+  EXPECT_GE(
+    lely::util::from_timespec(motor_driver_state_2.pos_timestamp) -
+      lely::util::from_timespec(motor_driver_state_1.pos_timestamp),
+    std::chrono::milliseconds(5));
+
+  EXPECT_LE(
+    lely::util::from_timespec(motor_driver_state_2.vel_current_timestamp) -
+      lely::util::from_timespec(motor_driver_state_1.vel_current_timestamp),
+    std::chrono::milliseconds(15));
+
+  EXPECT_GE(
+    lely::util::from_timespec(motor_driver_state_2.vel_current_timestamp) -
+      lely::util::from_timespec(motor_driver_state_1.vel_current_timestamp),
+    std::chrono::milliseconds(5));
+}
+
+TEST_F(TestRoboteqDriver, ReadDriverState)
+{
+  using panther_hardware_interfaces_test::DriverChannel;
+  using panther_hardware_interfaces_test::DriverFaultFlags;
+  using panther_hardware_interfaces_test::DriverRuntimeErrors;
+  using panther_hardware_interfaces_test::DriverScriptFlags;
+
+  const std::int16_t temp = 30;
+  const std::int16_t heatsink_temp = 31;
+  const std::uint16_t volt = 400;
+  const std::int16_t battery_current_1 = 10;
+  const std::int16_t battery_current_2 = 30;
+
+  mock_roboteq_->GetDriver()->SetTemperature(temp);
+  mock_roboteq_->GetDriver()->SetHeatsinkTemperature(heatsink_temp);
+  mock_roboteq_->GetDriver()->SetVoltage(volt);
+  mock_roboteq_->GetDriver()->SetBatteryCurrent1(battery_current_1);
+  mock_roboteq_->GetDriver()->SetBatteryCurrent2(battery_current_2);
+
+  mock_roboteq_->GetDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERHEAT);
+  mock_roboteq_->GetDriver()->SetDriverScriptFlag(DriverScriptFlags::ENCODER_DISCONNECTED);
+  mock_roboteq_->GetDriver()->SetDriverRuntimeError(
+    DriverChannel::CHANNEL1, DriverRuntimeErrors::LOOP_ERROR);
+  mock_roboteq_->GetDriver()->SetDriverRuntimeError(
+    DriverChannel::CHANNEL2, DriverRuntimeErrors::SAFETY_STOP_ACTIVE);
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+  panther_hardware_interfaces::DriverState driver_state = roboteq_driver_->ReadDriverState();
+
+  EXPECT_EQ(driver_state.mcu_temp, temp);
+  EXPECT_EQ(driver_state.heatsink_temp, heatsink_temp);
+  EXPECT_EQ(driver_state.battery_voltage, volt);
+  EXPECT_EQ(driver_state.battery_current_1, battery_current_1);
+  EXPECT_EQ(driver_state.battery_current_2, battery_current_2);
+
+  EXPECT_EQ(driver_state.fault_flags, 0b00000001);
+  EXPECT_EQ(driver_state.script_flags, 0b00000010);
+  EXPECT_EQ(driver_state.runtime_stat_flag_motor_1, 0b00000100);
+  EXPECT_EQ(driver_state.runtime_stat_flag_motor_2, 0b00001000);
+}
+
+TEST_F(TestRoboteqDriver, ReadDriverStateTimestamp)
+{
+  BootRoboteqDriver();
+
+  panther_hardware_interfaces::DriverState driver_state_1 = roboteq_driver_->ReadDriverState();
+
+  // based on publishing frequency in the Roboteq mock (20Hz)
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+  panther_hardware_interfaces::DriverState driver_state_2 = roboteq_driver_->ReadDriverState();
+
+  // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
+  // if consecutive messages will have timestamps 100ms + some threshold apart
+  EXPECT_LE(
+    lely::util::from_timespec(driver_state_2.flags_current_timestamp) -
+      lely::util::from_timespec(driver_state_1.flags_current_timestamp),
+    std::chrono::milliseconds(75));
+
+  EXPECT_GE(
+    lely::util::from_timespec(driver_state_2.flags_current_timestamp) -
+      lely::util::from_timespec(driver_state_1.flags_current_timestamp),
+    std::chrono::milliseconds(25));
+
+  EXPECT_LE(
+    lely::util::from_timespec(driver_state_2.voltages_temps_timestamp) -
+      lely::util::from_timespec(driver_state_1.voltages_temps_timestamp),
+    std::chrono::milliseconds(75));
+
+  EXPECT_GE(
+    lely::util::from_timespec(driver_state_2.voltages_temps_timestamp) -
+      lely::util::from_timespec(driver_state_1.voltages_temps_timestamp),
+    std::chrono::milliseconds(25));
+}
+
+TEST_F(TestRoboteqDriver, SendRoboteqCmd)
+{
+  using panther_hardware_interfaces_test::DriverChannel;
+
+  const std::int32_t motor_1_v = 10;
+  const std::int32_t motor_2_v = 20;
+
+  BootRoboteqDriver();
+
+  roboteq_driver_->GetMotorDriver(kMotor1Name)->SendCmdVel(motor_1_v);
+  roboteq_driver_->GetMotorDriver(kMotor2Name)->SendCmdVel(motor_2_v);
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+  EXPECT_EQ(mock_roboteq_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), motor_1_v);
+  EXPECT_EQ(mock_roboteq_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), motor_2_v);
+}
+
+TEST_F(TestRoboteqDriver, ResetRoboteqScript)
+{
+  BootRoboteqDriver();
+  mock_roboteq_->GetDriver()->SetResetRoboteqScript(65);
+  roboteq_driver_->ResetScript();
+
+  EXPECT_EQ(mock_roboteq_->GetDriver()->GetResetRoboteqScript(), 2);
+}
+
+TEST_F(TestRoboteqDriver, ResetRoboteqScriptSDOTimeoutReset)
+{
+  BootRoboteqDriver();
+
+  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 100000);
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { roboteq_driver_->ResetScript(); }, "SDO protocol timed out"));
+
+  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 0);
+  EXPECT_NO_THROW(roboteq_driver_->ResetScript());
+}
+
+TEST_F(TestRoboteqDriver, RoboteqTurnOnEStop)
+{
+  BootRoboteqDriver();
+  mock_roboteq_->GetDriver()->SetTurnOnEStop(65);
+  roboteq_driver_->TurnOnEStop();
+
+  EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOnEStop(), 1);
+}
+
+TEST_F(TestRoboteqDriver, TurnOnEStopTimeout)
+{
+  BootRoboteqDriver();
+  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200C, 0, 100000);
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { roboteq_driver_->TurnOnEStop(); }, "SDO protocol timed out"));
+}
+
+TEST_F(TestRoboteqDriver, TurnOffEStop)
+{
+  BootRoboteqDriver();
+  mock_roboteq_->GetDriver()->SetTurnOffEStop(65);
+  roboteq_driver_->TurnOffEStop();
+
+  EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOffEStop(), 1);
+}
+
+TEST_F(TestRoboteqDriver, TurnOffEStopTimeout)
+{
+  BootRoboteqDriver();
+  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200D, 0, 100000);
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { roboteq_driver_->TurnOffEStop(); }, "SDO protocol timed out"));
+}
+
+TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel1)
+{
+  BootRoboteqDriver();
+  mock_roboteq_->GetDriver()->SetTurnOnSafetyStop(67);
+  roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop();
+
+  EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOnSafetyStop(), 1);
+}
+
+TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel2)
+{
+  BootRoboteqDriver();
+  mock_roboteq_->GetDriver()->SetTurnOnSafetyStop(65);
+  roboteq_driver_->GetMotorDriver(kMotor2Name)->TurnOnSafetyStop();
+
+  EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOnSafetyStop(), 2);
+}
+
+TEST_F(TestRoboteqDriver, WriteTimeout)
+{
+  BootRoboteqDriver();
+  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 200000);
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop(); },
+    "SDO protocol timed out"));
+}
+
+// OnCanError/OnHeartbeatTimeout isn't tested, because it reacts to lower-level CAN errors (CRC),
+// which are hard to simulate, but it would be nice to add it
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+
+  auto result = RUN_ALL_TESTS();
+  return result;
+}
diff --git a/panther_hardware_interfaces/test/utils/fake_can_socket.hpp b/panther_hardware_interfaces/test/utils/fake_can_socket.hpp
new file mode 100644
index 00000000..e2920cff
--- /dev/null
+++ b/panther_hardware_interfaces/test/utils/fake_can_socket.hpp
@@ -0,0 +1,80 @@
+// 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_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_
+#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_
+
+#include <stdexcept>
+#include <string>
+
+#include <sys/socket.h>
+
+namespace panther_hardware_interfaces_test
+{
+
+class FakeCANSocket
+{
+public:
+  FakeCANSocket(const std::string & can_interface_name)
+  : can_interface_name_(can_interface_name), can_device_created_(false)
+  {
+  }
+
+  ~FakeCANSocket() { Deinitialize(); }
+
+  void Initialize()
+  {
+    if (system("sudo modprobe vcan") != 0) {
+      throw std::runtime_error("Failed to load vcan module");
+    }
+
+    // if link already exists, do not create it
+    const auto check_command = "ip link show " + can_interface_name_ + " > /dev/null 2>&1";
+    if (std::system(check_command.c_str()) != 0) {
+      const auto command = "sudo ip link add dev " + can_interface_name_ + " type vcan";
+      if (system(command.c_str()) != 0) {
+        throw std::runtime_error("Failed to add vcan device");
+      }
+    }
+
+    can_device_created_ = true;
+
+    const auto command = "sudo ip link set up " + can_interface_name_;
+    if (system(command.c_str()) != 0) {
+      throw std::runtime_error("Failed to set up vcan device");
+    }
+  }
+
+  void Deinitialize()
+  {
+    if (!can_device_created_) {
+      return;
+    }
+
+    std::string command = "sudo ip link delete " + can_interface_name_;
+    if (system(command.c_str()) != 0) {
+      throw std::runtime_error("Failed to delete vcan device");
+    }
+
+    can_device_created_ = false;
+  }
+
+private:
+  const std::string can_interface_name_;
+  bool can_device_created_;
+};
+
+}  // namespace panther_hardware_interfaces_test
+
+#endif  // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_
diff --git a/panther_hardware_interfaces/test/utils/roboteqs_mock.hpp b/panther_hardware_interfaces/test/utils/mock_roboteq.hpp
similarity index 85%
rename from panther_hardware_interfaces/test/utils/roboteqs_mock.hpp
rename to panther_hardware_interfaces/test/utils/mock_roboteq.hpp
index da6b4bb2..c863daf4 100644
--- a/panther_hardware_interfaces/test/utils/roboteqs_mock.hpp
+++ b/panther_hardware_interfaces/test/utils/mock_roboteq.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_TEST_UTILS_ROBOTEQ_MOCK_HPP_
-#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_ROBOTEQ_MOCK_HPP_
+#ifndef PANTHER_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_
+#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_
 
 #include <atomic>
 #include <condition_variable>
@@ -23,13 +23,13 @@
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
 
-#include <lely/coapp/slave.hpp>
-#include <lely/ev/loop.hpp>
-#include <lely/io2/linux/can.hpp>
-#include <lely/io2/posix/poll.hpp>
-#include <lely/io2/sys/io.hpp>
-#include <lely/io2/sys/sigset.hpp>
-#include <lely/io2/sys/timer.hpp>
+#include "lely/coapp/slave.hpp"
+#include "lely/ev/loop.hpp"
+#include "lely/io2/linux/can.hpp"
+#include "lely/io2/posix/poll.hpp"
+#include "lely/io2/sys/io.hpp"
+#include "lely/io2/sys/sigset.hpp"
+#include "lely/io2/sys/timer.hpp"
 
 namespace panther_hardware_interfaces_test
 {
@@ -258,16 +258,16 @@ class RoboteqSlave : public lely::canopen::BasicSlave
 };
 
 /**
- * @brief Class that simulates two Roboteqs
+ * @brief Class that simulates Roboteq controller
  */
-class RoboteqsMock
+class MockRoboteq
 {
 public:
-  RoboteqsMock() {}
-  ~RoboteqsMock() {}
+  MockRoboteq() {}
+  ~MockRoboteq() {}
 
   /**
-   * @brief Starts CAN communication and creates two simulated Roboteqs, that publish PDOs with set
+   * @brief Starts CAN communication and creates a simulated Roboteq, that publish PDOs with set
    * frequencies
    *
    * @param motors_states_period period of motors states publishing thread
@@ -291,11 +291,6 @@ class RoboteqsMock
             ament_index_cpp::get_package_share_directory("panther_hardware_interfaces")) /
           "test" / "config" / "slave_1.bin";
 
-        std::string slave2_eds_bin_path =
-          std::filesystem::path(
-            ament_index_cpp::get_package_share_directory("panther_hardware_interfaces")) /
-          "test" / "config" / "slave_2.bin";
-
         lely::io::IoGuard io_guard;
         lely::io::Poll poll(*ctx_);
         lely::ev::Loop loop(poll.get_poll());
@@ -307,21 +302,12 @@ class RoboteqsMock
         lely::io::CanChannel chan1(poll, exec);
         chan1.open(ctrl);
         lely::io::Timer timer1(poll, exec, CLOCK_MONOTONIC);
-        front_driver_ = std::make_shared<RoboteqSlave>(
+        driver_ = std::make_shared<RoboteqSlave>(
           timer1, chan1, slave_eds_path, slave1_eds_bin_path, 1);
 
-        lely::io::CanChannel chan2(poll, exec);
-        chan2.open(ctrl);
-        lely::io::Timer timer2(poll, exec, CLOCK_MONOTONIC);
-        rear_driver_ = std::make_shared<RoboteqSlave>(
-          timer2, chan2, slave_eds_path, slave2_eds_bin_path, 2);
-
-        front_driver_->Reset();
-        rear_driver_->Reset();
-        front_driver_->InitializeValues();
-        rear_driver_->InitializeValues();
-        front_driver_->StartPublishing(motors_states_period, driver_state_period);
-        rear_driver_->StartPublishing(motors_states_period, driver_state_period);
+        driver_->Reset();
+        driver_->InitializeValues();
+        driver_->StartPublishing(motors_states_period, driver_state_period);
 
         {
           std::lock_guard<std::mutex> lck_g(canopen_communication_started_mtx_);
@@ -331,8 +317,7 @@ class RoboteqsMock
 
         loop.run();
 
-        front_driver_->StopPublishing();
-        rear_driver_->StopPublishing();
+        driver_->StopPublishing();
       });
 
     if (!canopen_communication_started_.load()) {
@@ -346,7 +331,7 @@ class RoboteqsMock
   }
 
   /**
-   * @brief Stops CAN communication and removes simulated Roboteqs
+   * @brief Stops CAN communication and removes simulated Roboteq
    */
   void Stop()
   {
@@ -355,14 +340,12 @@ class RoboteqsMock
       canopen_communication_thread_.join();
     }
 
-    front_driver_.reset();
-    rear_driver_.reset();
+    driver_.reset();
 
     canopen_communication_started_.store(false);
   }
 
-  std::shared_ptr<RoboteqSlave> GetFrontDriver() { return front_driver_; }
-  std::shared_ptr<RoboteqSlave> GetRearDriver() { return rear_driver_; }
+  std::shared_ptr<RoboteqSlave> GetDriver() { return driver_; }
 
 private:
   std::shared_ptr<lely::io::Context> ctx_;
@@ -373,10 +356,9 @@ class RoboteqsMock
   std::condition_variable canopen_communication_started_cond_;
   std::mutex canopen_communication_started_mtx_;
 
-  std::shared_ptr<RoboteqSlave> front_driver_;
-  std::shared_ptr<RoboteqSlave> rear_driver_;
+  std::shared_ptr<RoboteqSlave> driver_;
 };
 
 }  // namespace panther_hardware_interfaces_test
 
-#endif  // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_ROBOTEQ_MOCK_HPP_
+#endif  // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_
diff --git a/panther_hardware_interfaces/test/utils/test_constants.hpp b/panther_hardware_interfaces/test/utils/test_constants.hpp
index be0d201e..ba1ff7ba 100644
--- a/panther_hardware_interfaces/test/utils/test_constants.hpp
+++ b/panther_hardware_interfaces/test/utils/test_constants.hpp
@@ -21,7 +21,7 @@
 #include <string>
 #include <vector>
 
-#include <panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp>
+#include <panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp>
 #include <panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp>
 
 namespace panther_hardware_interfaces_test

From 9769bbce721272dfbb37e8c25b5cbc76a4638fbb Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Mon, 2 Sep 2024 12:27:11 +0000
Subject: [PATCH 031/100] merge tests

---
 .../test_canopen_manager.cpp                  | 101 -----
 .../motors_controller/test_roboteq_driver.cpp | 390 ------------------
 .../robot_driver/test_canopen_manager.cpp     |  40 +-
 .../robot_driver/test_roboteq_driver.cpp      |  78 ++--
 4 files changed, 73 insertions(+), 536 deletions(-)
 delete mode 100644 panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp
 delete mode 100644 panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp

diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp
deleted file mode 100644
index 0291b5c8..00000000
--- a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_canopen_manager.cpp
+++ /dev/null
@@ -1,101 +0,0 @@
-// Copyright 2023 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 <memory>
-#include <stdexcept>
-#include <string>
-
-#include <gtest/gtest.h>
-#include <sys/socket.h>
-
-#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp"
-
-#include "utils/fake_can_socket.hpp"
-#include "utils/test_constants.hpp"
-
-#include "panther_utils/test/test_utils.hpp"
-
-class TestCANopenManager : public ::testing::Test
-{
-public:
-  TestCANopenManager();
-
-  ~TestCANopenManager() {}
-
-protected:
-  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
-  std::unique_ptr<panther_hardware_interfaces::CANopenManager> canopen_manager_;
-};
-
-TestCANopenManager::TestCANopenManager()
-{
-  can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
-    panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
-
-  canopen_manager_ = std::make_unique<panther_hardware_interfaces::CANopenManager>(
-    panther_hardware_interfaces_test::kCANopenSettings);
-}
-
-TEST_F(TestCANopenManager, InitializeAndDeinitialize)
-{
-  ASSERT_NO_THROW(canopen_manager_->Initialize());
-  ASSERT_NO_THROW(canopen_manager_->Deinitialize());
-
-  // Check if deinitialization worked correctly - initialize once again
-  ASSERT_NO_THROW(canopen_manager_->Initialize());
-}
-
-TEST_F(TestCANopenManager, Activate)
-{
-  can_socket_->Initialize();
-
-  ASSERT_NO_THROW(canopen_manager_->Initialize());
-  EXPECT_NO_THROW(canopen_manager_->Activate());
-  ASSERT_NO_THROW(canopen_manager_->Deinitialize());
-
-  // Check if deinitialization worked correctly - activate once again
-  ASSERT_NO_THROW(canopen_manager_->Initialize());
-  EXPECT_NO_THROW(canopen_manager_->Activate());
-}
-
-TEST_F(TestCANopenManager, ActivateNotInitialized)
-{
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [&]() { canopen_manager_->Activate(); }, "CANopenManager not initialized."));
-}
-
-TEST_F(TestCANopenManager, ActivateNoCommunication)
-{
-  ASSERT_NO_THROW(canopen_manager_->Initialize());
-  EXPECT_THROW(canopen_manager_->Activate(), std::runtime_error);
-}
-
-TEST_F(TestCANopenManager, GetMaster)
-{
-  can_socket_->Initialize();
-  ASSERT_NO_THROW(canopen_manager_->Initialize());
-  EXPECT_NO_THROW(canopen_manager_->GetMaster());
-}
-
-TEST_F(TestCANopenManager, GetMasterNotInitialized)
-{
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [&]() { canopen_manager_->GetMaster(); }, "CANopenManager not initialized."));
-}
-
-int main(int argc, char ** argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}
diff --git a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
deleted file mode 100644
index 8b02967a..00000000
--- a/panther_hardware_interfaces/test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
+++ /dev/null
@@ -1,390 +0,0 @@
-// Copyright 2023 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 <chrono>
-#include <cstdint>
-#include <memory>
-#include <string>
-#include <thread>
-
-#include <gtest/gtest.h>
-
-#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_manager.hpp"
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp"
-
-#include "utils/fake_can_socket.hpp"
-#include "utils/mock_roboteq.hpp"
-#include "utils/test_constants.hpp"
-
-#include "panther_utils/test/test_utils.hpp"
-
-class TestRoboteqDriver : public ::testing::Test
-{
-public:
-  TestRoboteqDriver();
-
-  ~TestRoboteqDriver();
-
-  void BootRoboteqDriver();
-
-protected:
-  bool TestBoot();
-
-  static constexpr char kMotor1Name[] = "motor_1";
-  static constexpr char kMotor2Name[] = "motor_2";
-
-  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
-  std::unique_ptr<panther_hardware_interfaces_test::MockRoboteq> mock_roboteq_;
-  std::unique_ptr<panther_hardware_interfaces::CANopenManager> canopen_manager_;
-  std::shared_ptr<panther_hardware_interfaces::RoboteqDriver> roboteq_driver_;
-};
-
-TestRoboteqDriver::TestRoboteqDriver()
-{
-  can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
-    panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
-  can_socket_->Initialize();
-
-  canopen_manager_ = std::make_unique<panther_hardware_interfaces::CANopenManager>(
-    panther_hardware_interfaces_test::kCANopenSettings);
-
-  mock_roboteq_ = std::make_unique<panther_hardware_interfaces_test::MockRoboteq>();
-  mock_roboteq_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
-  canopen_manager_->Initialize();
-
-  roboteq_driver_ = std::make_shared<panther_hardware_interfaces::RoboteqDriver>(
-    canopen_manager_->GetMaster(), 1, std::chrono::milliseconds(100));
-
-  canopen_manager_->Activate();
-}
-
-TestRoboteqDriver::~TestRoboteqDriver()
-{
-  roboteq_driver_.reset();
-  canopen_manager_->Deinitialize();
-  mock_roboteq_->Stop();
-  mock_roboteq_.reset();
-  can_socket_->Deinitialize();
-}
-
-void TestRoboteqDriver::BootRoboteqDriver()
-{
-  auto motor_1 = std::make_shared<panther_hardware_interfaces::RoboteqMotorDriver>(
-    roboteq_driver_, panther_hardware_interfaces::RoboteqDriver::kChannel1);
-  auto motor_2 = std::make_shared<panther_hardware_interfaces::RoboteqMotorDriver>(
-    roboteq_driver_, panther_hardware_interfaces::RoboteqDriver::kChannel2);
-
-  roboteq_driver_->AddMotorDriver(kMotor1Name, motor_1);
-  roboteq_driver_->AddMotorDriver(kMotor2Name, motor_2);
-  auto future = roboteq_driver_->Boot();
-  future.wait();
-}
-
-bool TestRoboteqDriver::TestBoot()
-{
-  auto future = roboteq_driver_->Boot();
-  const auto future_status = future.wait_for(std::chrono::milliseconds(500));
-
-  if (future_status != std::future_status::ready) {
-    return false;
-  }
-
-  try {
-    future.get();
-  } catch (const std::exception & e) {
-    return false;
-  }
-
-  return true;
-}
-
-TEST_F(TestRoboteqDriver, BootRoboteqDriver) { ASSERT_TRUE(TestBoot()); }
-
-TEST_F(TestRoboteqDriver, BootErrorDeviceType)
-{
-  const auto device_type_id = 0x1000;
-  const auto device_type_subid = 0;
-  mock_roboteq_->GetDriver()->SetOnReadWait<std::uint32_t>(
-    device_type_id, device_type_subid, 100000);
-  EXPECT_FALSE(TestBoot());
-}
-
-TEST_F(TestRoboteqDriver, BootErrorVendorId)
-{
-  const auto vendor_id_id = 0x1018;
-  const auto vendor_id_subid = 1;
-  mock_roboteq_->GetDriver()->SetOnReadWait<std::uint32_t>(vendor_id_id, vendor_id_subid, 100000);
-  EXPECT_FALSE(TestBoot());
-}
-
-TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
-{
-  using panther_hardware_interfaces_test::DriverChannel;
-
-  const std::int32_t motor_1_pos = 101;
-  const std::int32_t motor_1_vel = 102;
-  const std::int32_t motor_1_current = 103;
-  const std::int32_t motor_2_pos = 201;
-  const std::int32_t motor_2_vel = 202;
-  const std::int32_t motor_2_current = 203;
-
-  BootRoboteqDriver();
-
-  mock_roboteq_->GetDriver()->SetPosition(DriverChannel::CHANNEL1, motor_1_pos);
-  mock_roboteq_->GetDriver()->SetPosition(DriverChannel::CHANNEL2, motor_2_pos);
-
-  mock_roboteq_->GetDriver()->SetVelocity(DriverChannel::CHANNEL1, motor_1_vel);
-  mock_roboteq_->GetDriver()->SetVelocity(DriverChannel::CHANNEL2, motor_2_vel);
-
-  mock_roboteq_->GetDriver()->SetCurrent(DriverChannel::CHANNEL1, motor_1_current);
-  mock_roboteq_->GetDriver()->SetCurrent(DriverChannel::CHANNEL2, motor_2_current);
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(10));
-
-  panther_hardware_interfaces::MotorDriverState motor_driver_state_1 =
-    roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadMotorDriverState();
-  panther_hardware_interfaces::MotorDriverState motor_driver_state_2 =
-    roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadMotorDriverState();
-
-  EXPECT_EQ(motor_driver_state_1.pos, motor_1_pos);
-  EXPECT_EQ(motor_driver_state_1.vel, motor_1_vel);
-  EXPECT_EQ(motor_driver_state_1.current, motor_1_current);
-
-  EXPECT_EQ(motor_driver_state_2.pos, motor_2_pos);
-  EXPECT_EQ(motor_driver_state_2.vel, motor_2_vel);
-  EXPECT_EQ(motor_driver_state_2.current, motor_2_current);
-}
-
-TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps)
-{
-  BootRoboteqDriver();
-
-  panther_hardware_interfaces::MotorDriverState motor_driver_state_1 =
-    roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadMotorDriverState();
-
-  // based on publishing frequency in the Roboteq mock (100Hz)
-  std::this_thread::sleep_for(std::chrono::milliseconds(10));
-
-  panther_hardware_interfaces::MotorDriverState motor_driver_state_2 =
-    roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadMotorDriverState();
-
-  // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
-  // if consecutive messages will have timestamps 100ms + some threshold apart
-  EXPECT_LE(
-    lely::util::from_timespec(motor_driver_state_2.pos_timestamp) -
-      lely::util::from_timespec(motor_driver_state_1.pos_timestamp),
-    std::chrono::milliseconds(15));
-
-  EXPECT_GE(
-    lely::util::from_timespec(motor_driver_state_2.pos_timestamp) -
-      lely::util::from_timespec(motor_driver_state_1.pos_timestamp),
-    std::chrono::milliseconds(5));
-
-  EXPECT_LE(
-    lely::util::from_timespec(motor_driver_state_2.vel_current_timestamp) -
-      lely::util::from_timespec(motor_driver_state_1.vel_current_timestamp),
-    std::chrono::milliseconds(15));
-
-  EXPECT_GE(
-    lely::util::from_timespec(motor_driver_state_2.vel_current_timestamp) -
-      lely::util::from_timespec(motor_driver_state_1.vel_current_timestamp),
-    std::chrono::milliseconds(5));
-}
-
-TEST_F(TestRoboteqDriver, ReadDriverState)
-{
-  using panther_hardware_interfaces_test::DriverChannel;
-  using panther_hardware_interfaces_test::DriverFaultFlags;
-  using panther_hardware_interfaces_test::DriverRuntimeErrors;
-  using panther_hardware_interfaces_test::DriverScriptFlags;
-
-  const std::int16_t temp = 30;
-  const std::int16_t heatsink_temp = 31;
-  const std::uint16_t volt = 400;
-  const std::int16_t battery_current_1 = 10;
-  const std::int16_t battery_current_2 = 30;
-
-  mock_roboteq_->GetDriver()->SetTemperature(temp);
-  mock_roboteq_->GetDriver()->SetHeatsinkTemperature(heatsink_temp);
-  mock_roboteq_->GetDriver()->SetVoltage(volt);
-  mock_roboteq_->GetDriver()->SetBatteryCurrent1(battery_current_1);
-  mock_roboteq_->GetDriver()->SetBatteryCurrent2(battery_current_2);
-
-  mock_roboteq_->GetDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERHEAT);
-  mock_roboteq_->GetDriver()->SetDriverScriptFlag(DriverScriptFlags::ENCODER_DISCONNECTED);
-  mock_roboteq_->GetDriver()->SetDriverRuntimeError(
-    DriverChannel::CHANNEL1, DriverRuntimeErrors::LOOP_ERROR);
-  mock_roboteq_->GetDriver()->SetDriverRuntimeError(
-    DriverChannel::CHANNEL2, DriverRuntimeErrors::SAFETY_STOP_ACTIVE);
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(50));
-
-  panther_hardware_interfaces::DriverState driver_state = roboteq_driver_->ReadDriverState();
-
-  EXPECT_EQ(driver_state.mcu_temp, temp);
-  EXPECT_EQ(driver_state.heatsink_temp, heatsink_temp);
-  EXPECT_EQ(driver_state.battery_voltage, volt);
-  EXPECT_EQ(driver_state.battery_current_1, battery_current_1);
-  EXPECT_EQ(driver_state.battery_current_2, battery_current_2);
-
-  EXPECT_EQ(driver_state.fault_flags, 0b00000001);
-  EXPECT_EQ(driver_state.script_flags, 0b00000010);
-  EXPECT_EQ(driver_state.runtime_stat_flag_motor_1, 0b00000100);
-  EXPECT_EQ(driver_state.runtime_stat_flag_motor_2, 0b00001000);
-}
-
-TEST_F(TestRoboteqDriver, ReadDriverStateTimestamp)
-{
-  BootRoboteqDriver();
-
-  panther_hardware_interfaces::DriverState driver_state_1 = roboteq_driver_->ReadDriverState();
-
-  // based on publishing frequency in the Roboteq mock (20Hz)
-  std::this_thread::sleep_for(std::chrono::milliseconds(50));
-
-  panther_hardware_interfaces::DriverState driver_state_2 = roboteq_driver_->ReadDriverState();
-
-  // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
-  // if consecutive messages will have timestamps 100ms + some threshold apart
-  EXPECT_LE(
-    lely::util::from_timespec(driver_state_2.flags_current_timestamp) -
-      lely::util::from_timespec(driver_state_1.flags_current_timestamp),
-    std::chrono::milliseconds(75));
-
-  EXPECT_GE(
-    lely::util::from_timespec(driver_state_2.flags_current_timestamp) -
-      lely::util::from_timespec(driver_state_1.flags_current_timestamp),
-    std::chrono::milliseconds(25));
-
-  EXPECT_LE(
-    lely::util::from_timespec(driver_state_2.voltages_temps_timestamp) -
-      lely::util::from_timespec(driver_state_1.voltages_temps_timestamp),
-    std::chrono::milliseconds(75));
-
-  EXPECT_GE(
-    lely::util::from_timespec(driver_state_2.voltages_temps_timestamp) -
-      lely::util::from_timespec(driver_state_1.voltages_temps_timestamp),
-    std::chrono::milliseconds(25));
-}
-
-TEST_F(TestRoboteqDriver, SendRoboteqCmd)
-{
-  using panther_hardware_interfaces_test::DriverChannel;
-
-  const std::int32_t motor_1_v = 10;
-  const std::int32_t motor_2_v = 20;
-
-  BootRoboteqDriver();
-
-  roboteq_driver_->GetMotorDriver(kMotor1Name)->SendCmdVel(motor_1_v);
-  roboteq_driver_->GetMotorDriver(kMotor2Name)->SendCmdVel(motor_2_v);
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(50));
-
-  EXPECT_EQ(mock_roboteq_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), motor_1_v);
-  EXPECT_EQ(mock_roboteq_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), motor_2_v);
-}
-
-TEST_F(TestRoboteqDriver, ResetRoboteqScript)
-{
-  BootRoboteqDriver();
-  mock_roboteq_->GetDriver()->SetResetRoboteqScript(65);
-  roboteq_driver_->ResetScript();
-
-  EXPECT_EQ(mock_roboteq_->GetDriver()->GetResetRoboteqScript(), 2);
-}
-
-TEST_F(TestRoboteqDriver, ResetRoboteqScriptSDOTimeoutReset)
-{
-  BootRoboteqDriver();
-
-  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 100000);
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [&]() { roboteq_driver_->ResetScript(); }, "SDO protocol timed out"));
-
-  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 0);
-  EXPECT_NO_THROW(roboteq_driver_->ResetScript());
-}
-
-TEST_F(TestRoboteqDriver, RoboteqTurnOnEStop)
-{
-  BootRoboteqDriver();
-  mock_roboteq_->GetDriver()->SetTurnOnEStop(65);
-  roboteq_driver_->TurnOnEStop();
-
-  EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOnEStop(), 1);
-}
-
-TEST_F(TestRoboteqDriver, TurnOnEStopTimeout)
-{
-  BootRoboteqDriver();
-  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200C, 0, 100000);
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [&]() { roboteq_driver_->TurnOnEStop(); }, "SDO protocol timed out"));
-}
-
-TEST_F(TestRoboteqDriver, TurnOffEStop)
-{
-  BootRoboteqDriver();
-  mock_roboteq_->GetDriver()->SetTurnOffEStop(65);
-  roboteq_driver_->TurnOffEStop();
-
-  EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOffEStop(), 1);
-}
-
-TEST_F(TestRoboteqDriver, TurnOffEStopTimeout)
-{
-  BootRoboteqDriver();
-  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200D, 0, 100000);
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [&]() { roboteq_driver_->TurnOffEStop(); }, "SDO protocol timed out"));
-}
-
-TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel1)
-{
-  BootRoboteqDriver();
-  mock_roboteq_->GetDriver()->SetTurnOnSafetyStop(67);
-  roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop();
-
-  EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOnSafetyStop(), 1);
-}
-
-TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel2)
-{
-  BootRoboteqDriver();
-  mock_roboteq_->GetDriver()->SetTurnOnSafetyStop(65);
-  roboteq_driver_->GetMotorDriver(kMotor2Name)->TurnOnSafetyStop();
-
-  EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOnSafetyStop(), 2);
-}
-
-TEST_F(TestRoboteqDriver, WriteTimeout)
-{
-  BootRoboteqDriver();
-  mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 200000);
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [&]() { roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop(); },
-    "SDO protocol timed out"));
-}
-
-// OnCanError/OnHeartbeatTimeout isn't tested, because it reacts to lower-level CAN errors (CRC),
-// which are hard to simulate, but it would be nice to add it
-
-int main(int argc, char ** argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-
-  auto result = RUN_ALL_TESTS();
-  return result;
-}
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_canopen_manager.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_canopen_manager.cpp
index cf846219..51d5ec2b 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_canopen_manager.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_canopen_manager.cpp
@@ -24,6 +24,8 @@
 #include "utils/fake_can_socket.hpp"
 #include "utils/test_constants.hpp"
 
+#include "panther_utils/test/test_utils.hpp"
+
 class TestCANopenManager : public ::testing::Test
 {
 public:
@@ -47,25 +49,49 @@ TestCANopenManager::TestCANopenManager()
 
 TEST_F(TestCANopenManager, InitializeAndDeinitialize)
 {
-  can_socket_->Initialize();
-
   ASSERT_NO_THROW(canopen_manager_->Initialize());
   ASSERT_NO_THROW(canopen_manager_->Deinitialize());
 
   // Check if deinitialization worked correctly - initialize once again
   ASSERT_NO_THROW(canopen_manager_->Initialize());
-  ASSERT_NO_THROW(canopen_manager_->Deinitialize());
 }
 
-TEST_F(TestCANopenManager, InitializeWithError)
+TEST_F(TestCANopenManager, Activate)
 {
-  // CAN socket not initialized, should throw
-  ASSERT_THROW(canopen_manager_->Initialize(), std::runtime_error);
+  can_socket_->Initialize();
+
+  ASSERT_NO_THROW(canopen_manager_->Initialize());
+  EXPECT_NO_THROW(canopen_manager_->Activate());
   ASSERT_NO_THROW(canopen_manager_->Deinitialize());
 
+  // Check if deinitialization worked correctly - activate once again
+  ASSERT_NO_THROW(canopen_manager_->Initialize());
+  EXPECT_NO_THROW(canopen_manager_->Activate());
+}
+
+TEST_F(TestCANopenManager, ActivateNotInitialized)
+{
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { canopen_manager_->Activate(); }, "CANopenManager not initialized."));
+}
+
+TEST_F(TestCANopenManager, ActivateNoCommunication)
+{
+  ASSERT_NO_THROW(canopen_manager_->Initialize());
+  EXPECT_THROW(canopen_manager_->Activate(), std::runtime_error);
+}
+
+TEST_F(TestCANopenManager, GetMaster)
+{
   can_socket_->Initialize();
   ASSERT_NO_THROW(canopen_manager_->Initialize());
-  ASSERT_NO_THROW(canopen_manager_->Deinitialize());
+  EXPECT_NO_THROW(canopen_manager_->GetMaster());
+}
+
+TEST_F(TestCANopenManager, GetMasterNotInitialized)
+{
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { canopen_manager_->GetMaster(); }, "CANopenManager not initialized."));
 }
 
 int main(int argc, char ** argv)
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
index f0740627..a08da907 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
@@ -65,6 +65,8 @@ TestRoboteqDriver::TestRoboteqDriver()
 
   roboteq_driver_ = std::make_shared<panther_hardware_interfaces::RoboteqDriver>(
     canopen_manager_->GetMaster(), 1, std::chrono::milliseconds(100));
+
+  canopen_manager_->Activate();
 }
 
 TestRoboteqDriver::~TestRoboteqDriver()
@@ -150,53 +152,53 @@ TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
 
   std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
-  panther_hardware_interfaces::MotorDriverState fb_motor_1 =
+  panther_hardware_interfaces::MotorDriverState motor_driver_state_1 =
     roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadMotorDriverState();
-  panther_hardware_interfaces::MotorDriverState fb_motor_2 =
+  panther_hardware_interfaces::MotorDriverState motor_driver_state_2 =
     roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadMotorDriverState();
 
-  EXPECT_EQ(fb_motor_1.pos, motor_1_pos);
-  EXPECT_EQ(fb_motor_1.vel, motor_1_vel);
-  EXPECT_EQ(fb_motor_1.current, motor_1_current);
+  EXPECT_EQ(motor_driver_state_1.pos, motor_1_pos);
+  EXPECT_EQ(motor_driver_state_1.vel, motor_1_vel);
+  EXPECT_EQ(motor_driver_state_1.current, motor_1_current);
 
-  EXPECT_EQ(fb_motor_2.pos, motor_2_pos);
-  EXPECT_EQ(fb_motor_2.vel, motor_2_vel);
-  EXPECT_EQ(fb_motor_2.current, motor_2_current);
+  EXPECT_EQ(motor_driver_state_2.pos, motor_2_pos);
+  EXPECT_EQ(motor_driver_state_2.vel, motor_2_vel);
+  EXPECT_EQ(motor_driver_state_2.current, motor_2_current);
 }
 
 TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps)
 {
   BootRoboteqDriver();
 
-  panther_hardware_interfaces::MotorDriverState fb_motor_1 =
+  panther_hardware_interfaces::MotorDriverState motor_driver_state_1 =
     roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadMotorDriverState();
 
   // based on publishing frequency in the Roboteq mock (100Hz)
   std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
-  panther_hardware_interfaces::MotorDriverState fb_motor_2 =
+  panther_hardware_interfaces::MotorDriverState motor_driver_state_2 =
     roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadMotorDriverState();
 
   // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
   // if consecutive messages will have timestamps 100ms + some threshold apart
   EXPECT_LE(
-    lely::util::from_timespec(fb_motor_2.pos_timestamp) -
-      lely::util::from_timespec(fb_motor_1.pos_timestamp),
+    lely::util::from_timespec(motor_driver_state_2.pos_timestamp) -
+      lely::util::from_timespec(motor_driver_state_1.pos_timestamp),
     std::chrono::milliseconds(15));
 
   EXPECT_GE(
-    lely::util::from_timespec(fb_motor_2.pos_timestamp) -
-      lely::util::from_timespec(fb_motor_1.pos_timestamp),
+    lely::util::from_timespec(motor_driver_state_2.pos_timestamp) -
+      lely::util::from_timespec(motor_driver_state_1.pos_timestamp),
     std::chrono::milliseconds(5));
 
   EXPECT_LE(
-    lely::util::from_timespec(fb_motor_2.vel_current_timestamp) -
-      lely::util::from_timespec(fb_motor_1.vel_current_timestamp),
+    lely::util::from_timespec(motor_driver_state_2.vel_current_timestamp) -
+      lely::util::from_timespec(motor_driver_state_1.vel_current_timestamp),
     std::chrono::milliseconds(15));
 
   EXPECT_GE(
-    lely::util::from_timespec(fb_motor_2.vel_current_timestamp) -
-      lely::util::from_timespec(fb_motor_1.vel_current_timestamp),
+    lely::util::from_timespec(motor_driver_state_2.vel_current_timestamp) -
+      lely::util::from_timespec(motor_driver_state_1.vel_current_timestamp),
     std::chrono::milliseconds(5));
 }
 
@@ -228,51 +230,51 @@ TEST_F(TestRoboteqDriver, ReadDriverState)
 
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  panther_hardware_interfaces::DriverState fb = roboteq_driver_->ReadDriverState();
+  panther_hardware_interfaces::DriverState driver_state = roboteq_driver_->ReadDriverState();
 
-  EXPECT_EQ(fb.mcu_temp, temp);
-  EXPECT_EQ(fb.heatsink_temp, heatsink_temp);
-  EXPECT_EQ(fb.battery_voltage, volt);
-  EXPECT_EQ(fb.battery_current_1, battery_current_1);
-  EXPECT_EQ(fb.battery_current_2, battery_current_2);
+  EXPECT_EQ(driver_state.mcu_temp, temp);
+  EXPECT_EQ(driver_state.heatsink_temp, heatsink_temp);
+  EXPECT_EQ(driver_state.battery_voltage, volt);
+  EXPECT_EQ(driver_state.battery_current_1, battery_current_1);
+  EXPECT_EQ(driver_state.battery_current_2, battery_current_2);
 
-  EXPECT_EQ(fb.fault_flags, 0b00000001);
-  EXPECT_EQ(fb.script_flags, 0b00000010);
-  EXPECT_EQ(fb.runtime_stat_flag_channel_1, 0b00000100);
-  EXPECT_EQ(fb.runtime_stat_flag_channel_2, 0b00001000);
+  EXPECT_EQ(driver_state.fault_flags, 0b00000001);
+  EXPECT_EQ(driver_state.script_flags, 0b00000010);
+  EXPECT_EQ(driver_state.runtime_stat_flag_channel_1, 0b00000100);
+  EXPECT_EQ(driver_state.runtime_stat_flag_channel_2, 0b00001000);
 }
 
 TEST_F(TestRoboteqDriver, ReadDriverStateTimestamp)
 {
   BootRoboteqDriver();
 
-  panther_hardware_interfaces::DriverState fb1 = roboteq_driver_->ReadDriverState();
+  panther_hardware_interfaces::DriverState driver_state_1 = roboteq_driver_->ReadDriverState();
 
   // based on publishing frequency in the Roboteq mock (20Hz)
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  panther_hardware_interfaces::DriverState fb2 = roboteq_driver_->ReadDriverState();
+  panther_hardware_interfaces::DriverState driver_state_2 = roboteq_driver_->ReadDriverState();
 
   // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
   // if consecutive messages will have timestamps 100ms + some threshold apart
   EXPECT_LE(
-    lely::util::from_timespec(fb2.flags_current_timestamp) -
-      lely::util::from_timespec(fb1.flags_current_timestamp),
+    lely::util::from_timespec(driver_state_2.flags_current_timestamp) -
+      lely::util::from_timespec(driver_state_1.flags_current_timestamp),
     std::chrono::milliseconds(75));
 
   EXPECT_GE(
-    lely::util::from_timespec(fb2.flags_current_timestamp) -
-      lely::util::from_timespec(fb1.flags_current_timestamp),
+    lely::util::from_timespec(driver_state_2.flags_current_timestamp) -
+      lely::util::from_timespec(driver_state_1.flags_current_timestamp),
     std::chrono::milliseconds(25));
 
   EXPECT_LE(
-    lely::util::from_timespec(fb2.voltages_temps_timestamp) -
-      lely::util::from_timespec(fb1.voltages_temps_timestamp),
+    lely::util::from_timespec(driver_state_2.voltages_temps_timestamp) -
+      lely::util::from_timespec(driver_state_1.voltages_temps_timestamp),
     std::chrono::milliseconds(75));
 
   EXPECT_GE(
-    lely::util::from_timespec(fb2.voltages_temps_timestamp) -
-      lely::util::from_timespec(fb1.voltages_temps_timestamp),
+    lely::util::from_timespec(driver_state_2.voltages_temps_timestamp) -
+      lely::util::from_timespec(driver_state_1.voltages_temps_timestamp),
     std::chrono::milliseconds(25));
 }
 

From e11b9ea30288d9f33fea9c88a629b2bca42d0b7d Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Tue, 3 Sep 2024 10:51:56 +0200
Subject: [PATCH 032/100] Save work

---
 README.md                                     |   2 +-
 lynx_description/urdf/wheel.urdf.xacro        |   2 +-
 panther_bringup/CMakeLists.txt                |   2 +-
 panther_bringup/config/configuration.yaml     |  16 ++
 panther_bringup/launch/bringup.launch.py      |   6 +-
 .../launch/controller.launch.py               |  13 +-
 .../launch/load_urdf.launch.py                |   7 +-
 panther_gazebo/launch/spawn_robot.launch.py   |   6 +-
 .../panther_system_ros_interface.cpp          |   3 +-
 .../test_panther_system_ros_interface.cpp     |   7 +-
 panther_manager/src/safety_manager_node.cpp   |   3 +-
 panther_utils/panther_utils/arguments.py      | 142 ++++++++++++++++++
 .../{welcomeMsg.py => messages.py}            |   2 +-
 13 files changed, 181 insertions(+), 30 deletions(-)
 create mode 100644 panther_bringup/config/configuration.yaml
 create mode 100644 panther_utils/panther_utils/arguments.py
 rename panther_utils/panther_utils/{welcomeMsg.py => messages.py} (99%)

diff --git a/README.md b/README.md
index 633e44f3..a120c02a 100644
--- a/README.md
+++ b/README.md
@@ -108,7 +108,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | 🤖🖥️  | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                                             |
 | 🤖🖥️  | `user_led_animations_file`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                                                             |
 | 🤖🖥️  | `wheel_config_path`          | Path to wheel configuration file. <br/> ***string:*** [`{wheel_type}.yaml`](./panther_description/config)                                                                                                                                                                                                                                                   |
-| 🤖🖥️  | `wheel_type`                 | Type of wheel. If you choose a value from the preset options ('WH01', 'WH02', 'WH04'), you can ignore the 'wheel_config_path' and 'controller_config_path' parameters. For custom wheels, please define these parameters to point to files that accurately describe the custom wheels. <br/> ***string:*** `WH01` (choices: `WH01`, `WH02`, `WH04`, `custom`) |
+| 🤖🖥️  | `wheel_type`                 | Specify the wheel type. If the selected wheel type is not 'custom', the wheel_config_path and controller_config_path arguments will be automatically adjusted and can be omitted. <br/> ***string:*** `WH01` (choices: `WH01`, `WH02`, `WH04`, `custom`) |
 | 🖥️   | `x`                          | Initial robot position in the global 'x' axis. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                          |
 | 🖥️   | `y`                          | Initial robot position in the global 'y' axis. <br/> ***float:***` -2.0`                                                                                                                                                                                                                                                                                         |
 | 🖥️   | `z`                          | Initial robot position in the global 'z' axis. <br/> ***float:*** `0.2`                                                                                                                                                                                                                                                                                          |
diff --git a/lynx_description/urdf/wheel.urdf.xacro b/lynx_description/urdf/wheel.urdf.xacro
index 40dfbe54..7003ab3e 100644
--- a/lynx_description/urdf/wheel.urdf.xacro
+++ b/lynx_description/urdf/wheel.urdf.xacro
@@ -91,4 +91,4 @@
 
   </xacro:macro>
 
-</robot>
\ No newline at end of file
+</robot>
diff --git a/panther_bringup/CMakeLists.txt b/panther_bringup/CMakeLists.txt
index 79305750..121cdbd4 100644
--- a/panther_bringup/CMakeLists.txt
+++ b/panther_bringup/CMakeLists.txt
@@ -3,6 +3,6 @@ project(panther_bringup)
 
 find_package(ament_cmake REQUIRED)
 
-install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME})
 
 ament_package()
diff --git a/panther_bringup/config/configuration.yaml b/panther_bringup/config/configuration.yaml
new file mode 100644
index 00000000..8db2ac51
--- /dev/null
+++ b/panther_bringup/config/configuration.yaml
@@ -0,0 +1,16 @@
+# The minimal configuration file with the default values.
+# namespace is optional - without this element namespace will not be provided.
+
+# namespace:
+#   robot_model: panther
+#   init_pose: [0.0, 0.0, 0.0]
+#   init_rotation: [0.0, 0.0, 0.0]
+#   configuration:
+#     wheel_type: WH01
+
+panther:
+  robot_model: panther
+  init_pose: [0.0, 0.0, 0.0]
+  init_rotation: [0.0, 0.0, 0.0]
+  configuration:
+    wheel_type: WH01
diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py
index 96fc0445..ebb5bc1d 100644
--- a/panther_bringup/launch/bringup.launch.py
+++ b/panther_bringup/launch/bringup.launch.py
@@ -24,7 +24,7 @@
     PathJoinSubstitution,
 )
 from launch_ros.substitutions import FindPackageShare
-from panther_utils.welcomeMsg import welcomeMsg
+from panther_utils.messages import welcome_msg
 
 
 def generate_launch_description():
@@ -53,7 +53,7 @@ def generate_launch_description():
 
     serial_no = EnvironmentVariable(name="PANTHER_SERIAL_NO", default_value="----")
     panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0")
-    welcome_msg = welcomeMsg(serial_no, panther_version)
+    welcome_info = welcome_msg(serial_no, panther_version)
 
     controller_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -129,7 +129,7 @@ def generate_launch_description():
         declare_disable_manager_arg,
         declare_namespace_arg,
         declare_use_ekf_arg,
-        welcome_msg,
+        welcome_info,
         controller_launch,
         system_monitor_launch,
         delayed_action,
diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py
index a2212948..256c4ee3 100644
--- a/panther_controller/launch/controller.launch.py
+++ b/panther_controller/launch/controller.launch.py
@@ -125,10 +125,9 @@ def generate_launch_description():
         "wheel_type",
         default_value="WH01",
         description=(
-            "Type of wheel. If you choose a value from the preset options ('WH01', 'WH02',"
-            " 'WH04'), you can ignore the 'wheel_config_path' and 'controller_config_path'"
-            " parameters. For custom wheels, please define these parameters to point to files that"
-            " accurately describe the custom wheels."
+            "Specify the wheel type. If the selected wheel type is not 'custom', "
+            "the 'wheel_config_path' and 'controller_config_path' arguments will be "
+            "automatically adjusted and can be omitted."
         ),
         choices=["WH01", "WH02", "WH04", "custom"],
     )
@@ -219,8 +218,6 @@ def generate_launch_description():
             "controller_manager",
             "--controller-manager-timeout",
             "10",
-            "--namespace",
-            namespace,
         ],
         namespace=namespace,
         emulate_tty=True,
@@ -235,8 +232,6 @@ def generate_launch_description():
             "controller_manager",
             "--controller-manager-timeout",
             "10",
-            "--namespace",
-            namespace,
         ],
         namespace=namespace,
         emulate_tty=True,
@@ -259,8 +254,6 @@ def generate_launch_description():
             "controller_manager",
             "--controller-manager-timeout",
             "10",
-            "--namespace",
-            namespace,
         ],
         namespace=namespace,
         emulate_tty=True,
diff --git a/panther_description/launch/load_urdf.launch.py b/panther_description/launch/load_urdf.launch.py
index 13a0b83d..fbdd9aee 100644
--- a/panther_description/launch/load_urdf.launch.py
+++ b/panther_description/launch/load_urdf.launch.py
@@ -119,10 +119,9 @@ def generate_launch_description():
         "wheel_type",
         default_value="WH01",
         description=(
-            "Type of wheel. If you choose a value from the preset options ('WH01', 'WH02',"
-            " 'WH04'), you can ignore the 'wheel_config_path' and 'controller_config_path'"
-            " parameters. For custom wheels, please define these parameters to point to files that"
-            " accurately describe the custom wheels."
+            "Specify the wheel type. If the selected wheel type is not 'custom', "
+            "the 'wheel_config_path' and 'controller_config_path' arguments will be "
+            "automatically adjusted and can be omitted."
         ),
         choices=["WH01", "WH02", "WH04", "custom"],
     )
diff --git a/panther_gazebo/launch/spawn_robot.launch.py b/panther_gazebo/launch/spawn_robot.launch.py
index 945f92a3..0d43ba57 100644
--- a/panther_gazebo/launch/spawn_robot.launch.py
+++ b/panther_gazebo/launch/spawn_robot.launch.py
@@ -25,7 +25,7 @@
 )
 from launch_ros.actions import Node, SetUseSimTime
 from launch_ros.substitutions import FindPackageShare
-from panther_utils.welcomeMsg import welcomeMsg
+from panther_utils.messages import welcome_msg
 
 
 def generate_launch_description():
@@ -79,7 +79,7 @@ def generate_launch_description():
         "Robot namespace": namespace,
         "Initial pose": ["(", x, ", ", y, ", ", z, ", ", roll, ", ", pitch, ", ", yaw, ")"],
     }
-    welcome_msg = welcomeMsg("---", "simulation", log_stats)
+    welcome_info = welcome_msg("---", "simulation", log_stats)
 
     urdf_packages = PythonExpression(["'", robot, "_description'"])
     add_wheel_joints = LaunchConfiguration("add_wheel_joints", default="True")
@@ -134,7 +134,7 @@ def generate_launch_description():
             declare_pitch_arg,
             declare_yaw_arg,
             SetUseSimTime(True),
-            welcome_msg,
+            welcome_info,
             load_urdf,
             spawn_robot,
         ]
diff --git a/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp b/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp
index e6650784..9b2d2cbc 100644
--- a/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp
+++ b/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp
@@ -140,7 +140,8 @@ void PantherSystemRosInterface::UpdateMsgErrorFlags(
   front_driver_state.state.fault_flag = front.GetFaultFlag().GetMessage();
   front_driver_state.state.script_flag = front.GetScriptFlag().GetMessage();
   front_driver_state.state.channel_2_motor_runtime_error = front.GetLeftRuntimeError().GetMessage();
-  front_driver_state.state.channel_1_motor_runtime_error = front.GetRightRuntimeError().GetMessage();
+  front_driver_state.state.channel_1_motor_runtime_error =
+    front.GetRightRuntimeError().GetMessage();
 
   rear_driver_state.state.fault_flag = rear.GetFaultFlag().GetMessage();
   rear_driver_state.state.script_flag = rear.GetScriptFlag().GetMessage();
diff --git a/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp b/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
index bf522dce..5db85fc1 100644
--- a/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
+++ b/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
@@ -159,14 +159,15 @@ TEST_F(TestPantherSystemRosInterface, ErrorFlags)
 
   EXPECT_TRUE(driver_state_msg_->driver_states.at(0).state.fault_flag.overheat);
   EXPECT_TRUE(driver_state_msg_->driver_states.at(0).state.script_flag.encoder_disconnected);
-  EXPECT_TRUE(driver_state_msg_->driver_states.at(0).state.channel_2_motor_runtime_error.loop_error);
+  EXPECT_TRUE(
+    driver_state_msg_->driver_states.at(0).state.channel_2_motor_runtime_error.loop_error);
   EXPECT_TRUE(
     driver_state_msg_->driver_states.at(0).state.channel_1_motor_runtime_error.safety_stop_active);
 
   EXPECT_TRUE(driver_state_msg_->driver_states.at(1).state.fault_flag.overvoltage);
   EXPECT_TRUE(driver_state_msg_->driver_states.at(1).state.script_flag.loop_error);
-  EXPECT_TRUE(
-    driver_state_msg_->driver_states.at(1).state.channel_2_motor_runtime_error.forward_limit_triggered);
+  EXPECT_TRUE(driver_state_msg_->driver_states.at(1)
+                .state.channel_2_motor_runtime_error.forward_limit_triggered);
   EXPECT_TRUE(driver_state_msg_->driver_states.at(1)
                 .state.channel_1_motor_runtime_error.reverse_limit_triggered);
 }
diff --git a/panther_manager/src/safety_manager_node.cpp b/panther_manager/src/safety_manager_node.cpp
index 51c2dced..b0500684 100644
--- a/panther_manager/src/safety_manager_node.cpp
+++ b/panther_manager/src/safety_manager_node.cpp
@@ -82,8 +82,7 @@ void SafetyManagerNode::Initialize()
   battery_sub_ = this->create_subscription<BatteryStateMsg>(
     "battery/battery_status", 10, std::bind(&SafetyManagerNode::BatteryCB, this, _1));
   driver_state_sub_ = this->create_subscription<RobotDriverStateMsg>(
-    "hardware/robot_driver_state", 10,
-    std::bind(&SafetyManagerNode::RobotDriverStateCB, this, _1));
+    "hardware/robot_driver_state", 10, std::bind(&SafetyManagerNode::RobotDriverStateCB, this, _1));
   e_stop_sub_ = this->create_subscription<BoolMsg>(
     "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&SafetyManagerNode::EStopCB, this, _1));
diff --git a/panther_utils/panther_utils/arguments.py b/panther_utils/panther_utils/arguments.py
new file mode 100644
index 00000000..f7d3efb1
--- /dev/null
+++ b/panther_utils/panther_utils/arguments.py
@@ -0,0 +1,142 @@
+#!/usr/bin/env python3
+
+# Copyright 2018 Open Source Robotics Foundation, Inc.
+# 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.
+
+import sys
+from typing import Iterable, Tuple
+
+import yaml
+from launch import LaunchContext, Substitution
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import EnvironmentVariable, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+# Define valid configurations for each robot model
+VALID_CONFIGURATIONS = {
+    "panther": {"wheel_type": ["WH01", "WH02", "WH03"]},
+    "lynx": {"wheel_type": ["WH04"]},
+}
+
+
+def resolve_path(path: str | Substitution) -> str:
+    """Resolve a path that might be substitutions."""
+    if isinstance(path, Substitution):
+        return path.perform(LaunchContext())
+    return path
+
+
+def load_yaml_file(path: str) -> dict:
+    """Load YAML file and return its contents."""
+    try:
+        with open(path, "r") as file:
+            return yaml.safe_load(file)
+    except yaml.YAMLError as exc:
+        raise ValueError(f"Error reading YAML file: {exc}") from exc
+
+
+def load_robot_configuration(yaml_data: dict) -> Tuple[str, dict]:
+    """Retrieve first element data if exists; otherwise, return the base structure."""
+    if "configuration" not in yaml_data:
+        namespace = next(iter(yaml_data.keys()))
+        return namespace, yaml_data[namespace]
+    return "", yaml_data
+
+
+def validate_configuration(yaml_data: dict) -> None:
+    """Validate the robot model and wheel type configuration."""
+    robot_model = yaml_data.get("robot_model", "")
+    configuration_data = yaml_data.get("configuration", {})
+    wheel_type = configuration_data.get("wheel_type", "")
+
+    if robot_model not in VALID_CONFIGURATIONS:
+        raise ValueError(
+            f"Invalid robot model '{robot_model}'. "
+            f"Valid models are: {', '.join(VALID_CONFIGURATIONS.keys())}"
+        )
+
+    valid_wheel_types = VALID_CONFIGURATIONS[robot_model]["wheel_type"]
+    if wheel_type not in valid_wheel_types:
+        raise ValueError(
+            f"Invalid wheel type '{wheel_type}' for {robot_model}. "
+            f"Valid wheel types are: {', '.join(valid_wheel_types)}"
+        )
+
+
+def create_launch_arguments(namespace: str, yaml_data: dict) -> Iterable[DeclareLaunchArgument]:
+    """Generate ROS 2 launch description based on the YAML configuration."""
+    x, y, z = yaml_data.get("initial_pose", [0.0, 0.0, 0.0])
+    roll, pitch, yaw = yaml_data.get("initial_rotation", [0.0, 0.0, 0.0])
+    configuration_data = yaml_data.get("configuration", {})
+
+    return [
+        DeclareLaunchArgument(
+            "namespace",
+            default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=namespace),
+            description="Add namespace to all launched nodes.",
+        ),
+        DeclareLaunchArgument(
+            "robot_model",
+            default_value=EnvironmentVariable(
+                "ROBOT_MODEL", default_value=yaml_data.get("robot_model", "panther")
+            ),
+            description="Specify robot model type.",
+        ),
+        DeclareLaunchArgument(
+            "wheel_type",
+            default_value=configuration_data["wheel_type"],
+            description=(
+                "Specify the wheel type. If the selected wheel type is not 'custom', "
+                "the 'wheel_config_path' and 'controller_config_path' arguments will be "
+                "automatically adjusted and can be omitted."
+            ),
+        ),
+        DeclareLaunchArgument(
+            "x", default_value=x, description="Initial robot position in the global 'x' axis."
+        ),
+        DeclareLaunchArgument(
+            "y", default_value=y, description="Initial robot position in the global 'y' axis."
+        ),
+        DeclareLaunchArgument(
+            "z", default_value=z, description="Initial robot position in the global 'z' axis."
+        ),
+        DeclareLaunchArgument(
+            "roll", default_value=roll, description="Initial robot 'roll' orientation."
+        ),
+        DeclareLaunchArgument(
+            "pitch", default_value=pitch, description="Initial robot 'pitch' orientation."
+        ),
+        DeclareLaunchArgument(
+            "yaw", default_value=yaw, description="Initial robot 'yaw' orientation."
+        ),
+    ]
+
+
+def declare_robot_args(path: str | Substitution) -> Iterable[DeclareLaunchArgument]:
+    """Declare launch arguments based on the YAML configuration files."""
+    path = resolve_path(path)
+    yaml_data = load_yaml_file(path)
+    namespace, robot_config = load_robot_configuration(yaml_data)
+    try:
+        validate_configuration(robot_config)
+    except ValueError as error:
+        print(f"Validation Error: {error}")
+        sys.exit(1)
+    list_of_args = create_launch_arguments(namespace, robot_config)
+    return list_of_args
+
+
+path = PathJoinSubstitution([FindPackageShare("panther_bringup"), "config", "configuration.yaml"])
+print(declare_robot_args(path))
diff --git a/panther_utils/panther_utils/welcomeMsg.py b/panther_utils/panther_utils/messages.py
similarity index 99%
rename from panther_utils/panther_utils/welcomeMsg.py
rename to panther_utils/panther_utils/messages.py
index ae200ccd..4ada1f51 100644
--- a/panther_utils/panther_utils/welcomeMsg.py
+++ b/panther_utils/panther_utils/messages.py
@@ -33,7 +33,7 @@ def flatten(lst):
         return [lst]
 
 
-def welcomeMsg(
+def welcome_msg(
     serial_number: SomeSubstitutionsType,
     robot_hw_version: SomeSubstitutionsType,
     additional_stats: Dict = {},

From c056820895e70e7c9246c69c12cacc0595a831ad Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pawe=C5=82=20Irzyk?=
 <108666440+pawelirh@users.noreply.github.com>
Date: Wed, 4 Sep 2024 09:04:49 +0200
Subject: [PATCH 033/100] Ros2 lynx gpio tests (#401)

* Add common utility - version verification

* Implement GPIODriverInterface and GPIOControllerFactory

* Implement changes in GPIO controller tests

* Update GPIODriver tests

* Add throw info to gpio controller constructors
---
 ROS_API.md                                    |  12 +-
 lynx_description/urdf/wheel.urdf.xacro        |   2 +-
 panther_hardware_interfaces/CMakeLists.txt    |  21 ++-
 .../panther_system/gpio/gpio_controller.hpp   | 126 +++++++++----
 .../panther_system/gpio/gpio_driver.hpp       |  84 ++-------
 .../panther_system/gpio/types.hpp             |  88 +++++++++
 .../panther_system/panther_system.hpp         |   1 -
 panther_hardware_interfaces/package.xml       |   4 +
 .../panther_system/gpio/gpio_controller.cpp   |  60 +++++-
 .../src/panther_system/panther_system.cpp     |  15 +-
 .../gpio/test_gpio_controller.cpp             | 178 ++++++++++--------
 .../panther_system/gpio/test_gpio_driver.cpp  |  59 ++++--
 .../panther_utils/common_utilities.hpp        |  19 +-
 panther_utils/test/test_common_utilities.cpp  |  36 ++++
 14 files changed, 476 insertions(+), 229 deletions(-)
 create mode 100644 panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/types.hpp

diff --git a/ROS_API.md b/ROS_API.md
index a8c2ace8..17410767 100644
--- a/ROS_API.md
+++ b/ROS_API.md
@@ -1,12 +1,12 @@
 # ROS API
 
-> [!IMPORTANT]  
-> **Beta Release**  
->   
-> Please be advised that the software you are about to use is a Beta version of the ROS 2 Driver for Panther. It is functional, and the architecture will not change significantly. Although it has been tested by the Husarion team, some stability issues and bugs may still occur.  
->   
+> [!IMPORTANT]
+> **Beta Release**
+>
+> Please be advised that the software you are about to use is a Beta version of the ROS 2 Driver for Panther. It is functional, and the architecture will not change significantly. Although it has been tested by the Husarion team, some stability issues and bugs may still occur.
+>
 > We would greatly appreciate your feedback regarding the Panther ROS 2 driver. You can reach us in the following ways:
-> 
+>
 > - By email at: [support@husarion.com](mailto:support@husarion.com)
 > - Via our community forum: [Husarion Community](https://community.husarion.com)
 > - By submitting an issue request on: [GitHub](https://github.com/husarion/panther_ros/issues)
diff --git a/lynx_description/urdf/wheel.urdf.xacro b/lynx_description/urdf/wheel.urdf.xacro
index 40dfbe54..7003ab3e 100644
--- a/lynx_description/urdf/wheel.urdf.xacro
+++ b/lynx_description/urdf/wheel.urdf.xacro
@@ -91,4 +91,4 @@
 
   </xacro:macro>
 
-</robot>
\ No newline at end of file
+</robot>
diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt
index b87fda02..90bda757 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/panther_hardware_interfaces/CMakeLists.txt
@@ -84,6 +84,8 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(ros_testing REQUIRED)
   find_package(panther_utils REQUIRED)
 
   install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)
@@ -164,13 +166,7 @@ if(BUILD_TESTING)
   target_link_libraries(${PROJECT_NAME}_test_motors_controller
                         PkgConfig::LIBLELY_COAPP)
 
-  ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver
-                  test/panther_system/gpio/test_gpio_driver.cpp)
-  ament_target_dependencies(${PROJECT_NAME}_test_gpiod_driver panther_utils)
-  target_link_libraries(${PROJECT_NAME}_test_gpiod_driver ${PROJECT_NAME}
-                        PkgConfig::LIBGPIOD)
-
-  ament_add_gtest(
+  ament_add_gmock(
     ${PROJECT_NAME}_test_gpiod_controller
     test/panther_system/gpio/test_gpio_controller.cpp
     src/panther_system/gpio/gpio_controller.cpp
@@ -216,6 +212,17 @@ if(BUILD_TESTING)
   target_link_libraries(${PROJECT_NAME}_test_panther_system
                         PkgConfig::LIBLELY_COAPP)
 
+  # Integration tests
+  option(TEST_INTEGRATION "Run integration tests" OFF)
+  if(TEST_INTEGRATION)
+    # Hardware integration
+    ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver
+                    test/panther_system/gpio/test_gpio_driver.cpp)
+    ament_target_dependencies(${PROJECT_NAME}_test_gpiod_driver panther_utils)
+    target_link_libraries(${PROJECT_NAME}_test_gpiod_driver ${PROJECT_NAME}
+                          PkgConfig::LIBGPIOD)
+  endif()
+
 endif()
 
 ament_export_include_directories(include)
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp
index 993395a6..53315d8f 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp
@@ -29,7 +29,10 @@
 
 #include "gpiod.hpp"
 
+#include "panther_utils/common_utilities.hpp"
+
 #include "panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/gpio/types.hpp"
 
 namespace panther_hardware_interfaces
 {
@@ -54,7 +57,7 @@ class Watchdog
    * @exception std::runtime_error if the Watchdog pin is not configured by GPIODriver or not
    * described in GPIOController gpio_info storage
    */
-  Watchdog(std::shared_ptr<GPIODriver> gpio_driver);
+  Watchdog(std::shared_ptr<GPIODriverInterface> gpio_driver);
 
   /**
    * @brief Destructor for Watchdog class. Turns off the watchdog thread.
@@ -84,7 +87,7 @@ class Watchdog
   void WatchdogThread();
 
   GPIOPin watchdog_pin_ = GPIOPin::WATCHDOG;
-  std::shared_ptr<GPIODriver> gpio_driver_;
+  std::shared_ptr<GPIODriverInterface> gpio_driver_;
   std::thread watchdog_thread_;
   std::atomic_bool watchdog_thread_enabled_ = false;
 };
@@ -139,12 +142,20 @@ class GPIOControllerInterface
   bool IsPinAvailable(const GPIOPin pin) const;
 
 protected:
-  std::shared_ptr<GPIODriver> gpio_driver_;
+  std::shared_ptr<GPIODriverInterface> gpio_driver_;
 };
 
 class GPIOControllerPTH12X : public GPIOControllerInterface
 {
 public:
+  /**
+   * @brief Constructor for GPIOControllerPTH12X class.
+   *
+   * @param gpio_driver Pointer to the GPIODriver object.
+   * @throw `std::runtime_error` When the GPIO driver is not initialized.
+   */
+  GPIOControllerPTH12X(std::shared_ptr<GPIODriverInterface> gpio_driver);
+
   /**
    * @brief Initializes the GPIODriver, Watchdog, and powers on the motors.
    */
@@ -162,17 +173,19 @@ class GPIOControllerPTH12X : public GPIOControllerInterface
    * @brief Resets the E-stop.
    *
    * This method verifies the status of the E_STOP_RESET pin, which is configured as an input.
-   * If the pin is active, it attempts to reset the E-stop by momentarily setting it to an inactive
-   * state. During this reset process, the pin is configured as an output for a specific duration.
-   * If the attempt to reset the E-stop fails (the pin reads its value as an input again), it throws
-   * a runtime error. The Watchdog thread is temporarily activated during the E-stop reset process.
+   * If the pin is active, it attempts to reset the E-stop by momentarily setting it to an
+   * inactive state. During this reset process, the pin is configured as an output for a specific
+   * duration. If the attempt to reset the E-stop fails (the pin reads its value as an input
+   * again), it throws a runtime error. The Watchdog thread is temporarily activated during the
+   * E-stop reset process.
    * @return true if the E-stop is successfully reset.
    * @exception std::runtime_error when the E-stop reset fails.
    */
   void EStopReset() override;
 
   /**
-   * @brief Controls the motor power by enabling or disabling them based on the 'enable' parameter.
+   * @brief Controls the motor power by enabling or disabling them based on the 'enable'
+   * parameter.
    *
    * @param enable Set to 'true' to enable the motors, 'false' to disable.
    * @return 'true' if the motor control pin value is successfully set, 'false' otherwise.
@@ -204,7 +217,8 @@ class GPIOControllerPTH12X : public GPIOControllerInterface
   bool DigitalPowerEnable(const bool enable) override;
 
   /**
-   * @brief Enables or disables the use of an external charger according to the 'enable' parameter.
+   * @brief Enables or disables the use of an external charger according to the 'enable'
+   * parameter.
    *
    * @param enable Set to 'true' to enable external charger, 'false' to disable.
    * @return 'true' if the charger control pin value is successfully set, 'false' otherwise.
@@ -226,6 +240,11 @@ class GPIOControllerPTH12X : public GPIOControllerInterface
    */
   std::unordered_map<GPIOPin, bool> QueryControlInterfaceIOStates() const override;
 
+  /**
+   * @brief Returns the GPIO pin configuration information for the PTH12X.
+   */
+  static const std::vector<GPIOInfo> & GetGPIOConfigInfoStorage();
+
   void InterruptEStopReset() override;
 
 protected:
@@ -235,9 +254,9 @@ class GPIOControllerPTH12X : public GPIOControllerInterface
   /**
    * @brief Waits for a specific duration or until an interruption is signaled.
    *
-   * This method is designed to block execution for the specified timeout duration. It also monitors
-   * for an interruption signal which, if received, will cause the method to return early. The
-   * interruption is controlled by the `should_abort_e_stop_reset_` flag.
+   * This method is designed to block execution for the specified timeout duration. It also
+   * monitors for an interruption signal which, if received, will cause the method to return
+   * early. The interruption is controlled by the `should_abort_e_stop_reset_` flag.
    *
    * @param timeout Duration to wait for in milliseconds.
    * @return `true` if the wait completed without interruption, `false` if an interruption was
@@ -246,25 +265,10 @@ class GPIOControllerPTH12X : public GPIOControllerInterface
   bool WaitFor(std::chrono::milliseconds timeout);
 
   /**
-   * @brief Vector containing GPIO pin configuration information such as pin direction, value, etc.
-   */
-  const std::vector<GPIOInfo> gpio_config_info_storage_{
-    GPIOInfo{GPIOPin::WATCHDOG, gpiod::line::direction::OUTPUT},
-    GPIOInfo{GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT},
-    GPIOInfo{GPIOPin::CHRG_DISABLE, gpiod::line::direction::OUTPUT},
-    GPIOInfo{GPIOPin::DRIVER_EN, gpiod::line::direction::OUTPUT},
-    GPIOInfo{GPIOPin::E_STOP_RESET, gpiod::line::direction::INPUT},
-    GPIOInfo{GPIOPin::FAN_SW, gpiod::line::direction::OUTPUT},
-    GPIOInfo{GPIOPin::GPOUT1, gpiod::line::direction::OUTPUT},
-    GPIOInfo{GPIOPin::GPOUT2, gpiod::line::direction::OUTPUT},
-    GPIOInfo{GPIOPin::GPIN1, gpiod::line::direction::INPUT},
-    GPIOInfo{GPIOPin::GPIN2, gpiod::line::direction::INPUT},
-    GPIOInfo{GPIOPin::SHDN_INIT, gpiod::line::direction::INPUT},
-    GPIOInfo{GPIOPin::VDIG_OFF, gpiod::line::direction::OUTPUT},
-    GPIOInfo{GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT},
-    GPIOInfo{GPIOPin::CHRG_SENSE, gpiod::line::direction::INPUT, true},
-    GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true},
-  };
+   * @brief Vector containing GPIO pin configuration information such as pin direction, value,
+   * etc.
+   */
+  static const std::vector<GPIOInfo> gpio_config_info_storage_;
 
   std::mutex e_stop_cv_mtx_;
   std::condition_variable e_stop_cv_;
@@ -274,6 +278,14 @@ class GPIOControllerPTH12X : public GPIOControllerInterface
 class GPIOControllerPTH10X : public GPIOControllerInterface
 {
 public:
+  /**
+   * @brief Constructor for GPIOControllerPTH10X class.
+   *
+   * @param gpio_driver Pointer to the GPIODriver object.
+   * @throw `std::runtime_error` When the GPIO driver is not initialized.
+   */
+  GPIOControllerPTH10X(std::shared_ptr<GPIODriverInterface> gpio_driver);
+
   /**
    * @brief Initializes the GPIODriver and powers on the motors.
    */
@@ -297,7 +309,8 @@ class GPIOControllerPTH10X : public GPIOControllerInterface
   void EStopReset() override;
 
   /**
-   * @brief Controls the motor power by enabling or disabling them based on the 'enable' parameter.
+   * @brief Controls the motor power by enabling or disabling them based on the 'enable'
+   * parameter.
    *
    * This method checks if the motors are powered up by verifying the 'STAGE2_INPUT' pin.
    *
@@ -333,8 +346,8 @@ class GPIOControllerPTH10X : public GPIOControllerInterface
    * robot version.
    *
    * @param enable Ignored parameter in this version.
-   * @exception std::runtime_error Always throws a runtime error due to lack of support for digital
-   * power control.
+   * @exception std::runtime_error Always throws a runtime error due to lack of support for
+   * digital power control.
    */
   bool DigitalPowerEnable(const bool /* enable */) override;
 
@@ -343,8 +356,8 @@ class GPIOControllerPTH10X : public GPIOControllerInterface
    * robot version.
    *
    * @param enable Ignored parameter in this version.
-   * @exception std::runtime_error Always throws a runtime error due to lack of support for charging
-   * process control.
+   * @exception std::runtime_error Always throws a runtime error due to lack of support for
+   * charging process control.
    */
   bool ChargerEnable(const bool /* enable */) override;
 
@@ -364,14 +377,45 @@ class GPIOControllerPTH10X : public GPIOControllerInterface
    */
   std::unordered_map<GPIOPin, bool> QueryControlInterfaceIOStates() const override;
 
+  /**
+   * @brief Returns the GPIO pin configuration information for the PTH10X.
+   */
+  static const std::vector<GPIOInfo> & GetGPIOConfigInfoStorage();
+
 private:
   /**
-   * @brief Vector containing GPIO pin configuration information such as pin direction, value, etc.
+   * @brief Vector containing GPIO pin configuration information such as pin direction, value,
+   * etc.
    */
-  const std::vector<GPIOInfo> gpio_config_info_storage_{
-    GPIOInfo{GPIOPin::STAGE2_INPUT, gpiod::line::direction::INPUT},
-    GPIOInfo{GPIOPin::MOTOR_ON, gpiod::line::direction::OUTPUT},
-    GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true},
+  static const std::vector<GPIOInfo> gpio_config_info_storage_;
+};
+
+class GPIOControllerFactory
+{
+public:
+  /**
+   * @brief Creates a GPIO controller based on the robot version.
+   *
+   * @param robot_version The robot version to create the GPIO controller for.
+   * @return A unique pointer to the created GPIO controller.
+   */
+  static std::unique_ptr<GPIOControllerInterface> CreateGPIOController(const float robot_version)
+  {
+    std::unique_ptr<GPIOControllerInterface> gpio_controller;
+
+    if (panther_utils::common_utilities::MeetsVersionRequirement(robot_version, 1.2)) {
+      auto config_info_storage = GPIOControllerPTH12X::GetGPIOConfigInfoStorage();
+      auto gpio_driver = std::make_shared<GPIODriver>(config_info_storage);
+
+      gpio_controller = std::make_unique<GPIOControllerPTH12X>(gpio_driver);
+    } else {
+      auto config_info_storage = GPIOControllerPTH10X::GetGPIOConfigInfoStorage();
+      auto gpio_driver = std::make_shared<GPIODriver>(config_info_storage);
+
+      gpio_controller = std::make_unique<GPIOControllerPTH10X>(gpio_driver);
+    }
+
+    return gpio_controller;
   };
 };
 
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp
index d71d81c9..e46775fc 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp
@@ -29,51 +29,30 @@
 
 #include "gpiod.hpp"
 
+#include "panther_hardware_interfaces/panther_system/gpio/types.hpp"
+
 namespace panther_hardware_interfaces
 {
 
-/**
- * @brief Enumeration representing available GPIO pins in the Panther system.
- */
-enum class GPIOPin {
-  AUX_PW_EN,
-  CHRG_DISABLE,
-  CHRG_SENSE,
-  DRIVER_EN,
-  E_STOP_RESET,
-  FAN_SW,
-  GPOUT1,
-  GPOUT2,
-  GPIN1,
-  GPIN2,
-  LED_SBC_SEL,
-  SHDN_INIT,
-  STAGE2_INPUT,
-  VDIG_OFF,
-  VMOT_ON,
-  MOTOR_ON,
-  WATCHDOG
-};
-
-/**
- * @brief Structure containing information related to GPIO pins such as pin configuration,
- * direction, value, etc. This information is required during the initialization process.
- */
-struct GPIOInfo
+class GPIODriverInterface
 {
-  GPIOPin pin;
-  gpiod::line::direction direction;
-  bool active_low = false;
-  gpiod::line::value init_value = gpiod::line::value::INACTIVE;
-  gpiod::line::value value = gpiod::line::value::INACTIVE;
-  gpiod::line::offset offset = -1;
+public:
+  virtual ~GPIODriverInterface() = default;
+  virtual void GPIOMonitorEnable(
+    const bool use_rt = false, const unsigned gpio_monit_thread_sched_priority = 60) = 0;
+  virtual void ConfigureEdgeEventCallback(
+    const std::function<void(const GPIOInfo &)> & callback) = 0;
+  virtual void ChangePinDirection(const GPIOPin pin, const gpiod::line::direction direction) = 0;
+  virtual bool IsPinAvailable(const GPIOPin pin) const = 0;
+  virtual bool IsPinActive(const GPIOPin pin) = 0;
+  virtual bool SetPinValue(const GPIOPin pin, const bool value) = 0;
 };
 
 /**
  * @brief Class responsible for managing GPIO pins on Panther robots, handling tasks such as
  * setting pin values, changing pin directions, monitoring pin events, and more.
  */
-class GPIODriver
+class GPIODriver : public GPIODriverInterface
 {
 public:
   /**
@@ -127,7 +106,7 @@ class GPIODriver
    *       lack of functionality to read pin values.
    */
   void GPIOMonitorEnable(
-    const bool use_rt = false, const unsigned gpio_monit_thread_sched_priority = 60);
+    const bool use_rt = false, const unsigned gpio_monit_thread_sched_priority = 60) override;
 
   /**
    * @brief This method sets the provided callback function to be executed upon GPIO edge events.
@@ -154,7 +133,7 @@ class GPIODriver
    *     std::bind(&MyClass::HandleGPIOEvent, &my_obj, std::placeholders::_1));
    * @endcode
    */
-  void ConfigureEdgeEventCallback(const std::function<void(const GPIOInfo &)> & callback);
+  void ConfigureEdgeEventCallback(const std::function<void(const GPIOInfo &)> & callback) override;
 
   /**
    * @brief Changes the direction of a specific GPIO pin.
@@ -162,7 +141,7 @@ class GPIODriver
    * @param pin GPIOPin to change the direction for.
    * @param direction New direction for the pin.
    */
-  void ChangePinDirection(const GPIOPin pin, const gpiod::line::direction direction);
+  void ChangePinDirection(const GPIOPin pin, const gpiod::line::direction direction) override;
 
   /**
    * @brief Returns true if a specific pin is configured and stored in GPIO info storage
@@ -170,7 +149,7 @@ class GPIODriver
    * @param pin The GPIO pin to check availability for
    * @return true if the pin is available, false otherwise
    */
-  bool IsPinAvailable(const GPIOPin pin) const;
+  bool IsPinAvailable(const GPIOPin pin) const override;
 
   /**
    * @brief Checks if a specific GPIO pin is active. This method returns the value stored in the
@@ -182,7 +161,7 @@ class GPIODriver
    *
    * @return True if the pin is active, false otherwise.
    */
-  bool IsPinActive(const GPIOPin pin);
+  bool IsPinActive(const GPIOPin pin) override;
 
   /**
    * @brief Sets the value for a specific GPIO pin.
@@ -195,7 +174,7 @@ class GPIODriver
    *
    * @return true if the pin value is successfully set, false otherwise.
    */
-  bool SetPinValue(const GPIOPin pin, const bool value);
+  bool SetPinValue(const GPIOPin pin, const bool value) override;
 
 private:
   std::unique_ptr<gpiod::line_request> CreateLineRequest(gpiod::chip & chip);
@@ -228,29 +207,6 @@ class GPIODriver
    */
   std::function<void(const GPIOInfo & gpio_info)> GPIOEdgeEventCallback;
 
-  /**
-   * @brief Mapping of GPIO pins to their respective names.
-   */
-  const std::map<GPIOPin, std::string> pin_names_{
-    {GPIOPin::WATCHDOG, "WATCHDOG"},
-    {GPIOPin::AUX_PW_EN, "AUX_PW_EN"},
-    {GPIOPin::CHRG_DISABLE, "CHRG_DISABLE"},
-    {GPIOPin::CHRG_SENSE, "CHRG_SENSE"},
-    {GPIOPin::DRIVER_EN, "DRIVER_EN"},
-    {GPIOPin::E_STOP_RESET, "E_STOP_RESET"},
-    {GPIOPin::FAN_SW, "FAN_SW"},
-    {GPIOPin::GPOUT1, "GPOUT1"},
-    {GPIOPin::GPOUT2, "GPOUT2"},
-    {GPIOPin::GPIN1, "GPIN1"},
-    {GPIOPin::GPIN2, "GPIN2"},
-    {GPIOPin::LED_SBC_SEL, "LED_SBC_SEL"},
-    {GPIOPin::SHDN_INIT, "SHDN_INIT"},
-    {GPIOPin::STAGE2_INPUT, "STAGE2_INPUT"},
-    {GPIOPin::VDIG_OFF, "VDIG_OFF"},
-    {GPIOPin::VMOT_ON, "VMOT_ON"},
-    {GPIOPin::MOTOR_ON, "MOTOR_ON"},
-  };
-
   /**
    * @brief Vector containing GPIO pin configuration information such as pin direction, value, etc.
    */
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/types.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/types.hpp
new file mode 100644
index 00000000..9695a8fd
--- /dev/null
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/types.hpp
@@ -0,0 +1,88 @@
+// 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_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_TYPES_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_TYPES_HPP_
+
+#include <map>
+#include <string>
+
+#include "gpiod.hpp"
+
+namespace panther_hardware_interfaces
+{
+
+/**
+ * @brief Enumeration representing available GPIO pins in the Panther system.
+ */
+enum class GPIOPin {
+  AUX_PW_EN,
+  CHRG_DISABLE,
+  CHRG_SENSE,
+  DRIVER_EN,
+  E_STOP_RESET,
+  FAN_SW,
+  GPOUT1,
+  GPOUT2,
+  GPIN1,
+  GPIN2,
+  LED_SBC_SEL,
+  SHDN_INIT,
+  STAGE2_INPUT,
+  VDIG_OFF,
+  VMOT_ON,
+  MOTOR_ON,
+  WATCHDOG
+};
+
+/**
+ * @brief Mapping of GPIO pins to their respective names.
+ */
+const std::map<GPIOPin, std::string> pin_names_{
+  {GPIOPin::WATCHDOG, "WATCHDOG"},
+  {GPIOPin::AUX_PW_EN, "AUX_PW_EN"},
+  {GPIOPin::CHRG_DISABLE, "CHRG_DISABLE"},
+  {GPIOPin::CHRG_SENSE, "CHRG_SENSE"},
+  {GPIOPin::DRIVER_EN, "DRIVER_EN"},
+  {GPIOPin::E_STOP_RESET, "E_STOP_RESET"},
+  {GPIOPin::FAN_SW, "FAN_SW"},
+  {GPIOPin::GPOUT1, "GPOUT1"},
+  {GPIOPin::GPOUT2, "GPOUT2"},
+  {GPIOPin::GPIN1, "GPIN1"},
+  {GPIOPin::GPIN2, "GPIN2"},
+  {GPIOPin::LED_SBC_SEL, "LED_SBC_SEL"},
+  {GPIOPin::SHDN_INIT, "SHDN_INIT"},
+  {GPIOPin::STAGE2_INPUT, "STAGE2_INPUT"},
+  {GPIOPin::VDIG_OFF, "VDIG_OFF"},
+  {GPIOPin::VMOT_ON, "VMOT_ON"},
+  {GPIOPin::MOTOR_ON, "MOTOR_ON"},
+};
+
+/**
+ * @brief Structure containing information related to GPIO pins such as pin configuration,
+ * direction, value, etc. This information is required during the initialization process.
+ */
+struct GPIOInfo
+{
+  GPIOPin pin;
+  gpiod::line::direction direction;
+  bool active_low = false;
+  gpiod::line::value init_value = gpiod::line::value::INACTIVE;
+  gpiod::line::value value = gpiod::line::value::INACTIVE;
+  gpiod::line::offset offset = -1;
+};
+
+}  // namespace panther_hardware_interfaces
+
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_TYPES_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp
index 362be65b..373ff0ee 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp
@@ -90,7 +90,6 @@ class PantherSystem : public hardware_interface::SystemInterface
   void UpdateHwStates();
   void UpdateMotorsStateDataTimedOut();
   bool AreVelocityCommandsNearZero();
-  bool IsPantherVersionAtLeast(const float version);
 
   void UpdateDriverStateMsg();
   void UpdateFlagErrors();
diff --git a/panther_hardware_interfaces/package.xml b/panther_hardware_interfaces/package.xml
index df075d21..f44f0b30 100644
--- a/panther_hardware_interfaces/package.xml
+++ b/panther_hardware_interfaces/package.xml
@@ -43,6 +43,10 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>google-mock</test_depend>
+  <test_depend>ros_testing</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>
diff --git a/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp b/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp
index fe13fcd8..7bfbf24c 100644
--- a/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp
+++ b/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp
@@ -24,10 +24,14 @@
 
 #include "gpiod.hpp"
 
+#include "panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/gpio/types.hpp"
+
 namespace panther_hardware_interfaces
 {
 
-Watchdog::Watchdog(std::shared_ptr<GPIODriver> gpio_driver) : gpio_driver_(std::move(gpio_driver))
+Watchdog::Watchdog(std::shared_ptr<GPIODriverInterface> gpio_driver)
+: gpio_driver_(std::move(gpio_driver))
 {
   if (!gpio_driver_->IsPinAvailable(watchdog_pin_)) {
     throw std::runtime_error("Watchdog pin is not configured.");
@@ -98,9 +102,17 @@ bool GPIOControllerInterface::IsPinAvailable(const GPIOPin pin) const
   return gpio_driver_->IsPinAvailable(pin);
 }
 
+GPIOControllerPTH12X::GPIOControllerPTH12X(std::shared_ptr<GPIODriverInterface> gpio_driver)
+{
+  gpio_driver_ = gpio_driver;
+
+  if (!gpio_driver_) {
+    throw std::runtime_error("GPIO driver is not initialized.");
+  }
+}
+
 void GPIOControllerPTH12X::Start()
 {
-  gpio_driver_ = std::make_shared<GPIODriver>(gpio_config_info_storage_);
   gpio_driver_->GPIOMonitorEnable(true, 60);
 
   gpio_driver_->SetPinValue(GPIOPin::VMOT_ON, true);
@@ -197,6 +209,29 @@ std::unordered_map<GPIOPin, bool> GPIOControllerPTH12X::QueryControlInterfaceIOS
   return io_state;
 }
 
+const std::vector<GPIOInfo> GPIOControllerPTH12X::gpio_config_info_storage_ = {
+  GPIOInfo{GPIOPin::WATCHDOG, gpiod::line::direction::OUTPUT},
+  GPIOInfo{GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT},
+  GPIOInfo{GPIOPin::CHRG_DISABLE, gpiod::line::direction::OUTPUT},
+  GPIOInfo{GPIOPin::DRIVER_EN, gpiod::line::direction::OUTPUT},
+  GPIOInfo{GPIOPin::E_STOP_RESET, gpiod::line::direction::INPUT},
+  GPIOInfo{GPIOPin::FAN_SW, gpiod::line::direction::OUTPUT},
+  GPIOInfo{GPIOPin::GPOUT1, gpiod::line::direction::OUTPUT},
+  GPIOInfo{GPIOPin::GPOUT2, gpiod::line::direction::OUTPUT},
+  GPIOInfo{GPIOPin::GPIN1, gpiod::line::direction::INPUT},
+  GPIOInfo{GPIOPin::GPIN2, gpiod::line::direction::INPUT},
+  GPIOInfo{GPIOPin::SHDN_INIT, gpiod::line::direction::INPUT},
+  GPIOInfo{GPIOPin::VDIG_OFF, gpiod::line::direction::OUTPUT},
+  GPIOInfo{GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT},
+  GPIOInfo{GPIOPin::CHRG_SENSE, gpiod::line::direction::INPUT, true},
+  GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true},
+};
+
+const std::vector<GPIOInfo> & GPIOControllerPTH12X::GetGPIOConfigInfoStorage()
+{
+  return gpio_config_info_storage_;
+}
+
 void GPIOControllerPTH12X::InterruptEStopReset()
 {
   std::lock_guard<std::mutex> lck(e_stop_cv_mtx_);
@@ -215,9 +250,17 @@ bool GPIOControllerPTH12X::WaitFor(std::chrono::milliseconds timeout)
   return !interrupted;
 }
 
+GPIOControllerPTH10X::GPIOControllerPTH10X(std::shared_ptr<GPIODriverInterface> gpio_driver)
+{
+  gpio_driver_ = gpio_driver;
+
+  if (!gpio_driver_) {
+    throw std::runtime_error("GPIO driver is not initialized.");
+  }
+}
+
 void GPIOControllerPTH10X::Start()
 {
-  gpio_driver_ = std::make_shared<GPIODriver>(gpio_config_info_storage_);
   gpio_driver_->GPIOMonitorEnable(true, 60);
 
   gpio_driver_->SetPinValue(GPIOPin::MOTOR_ON, true);
@@ -288,4 +331,15 @@ std::unordered_map<GPIOPin, bool> GPIOControllerPTH10X::QueryControlInterfaceIOS
   return io_state;
 }
 
+const std::vector<GPIOInfo> GPIOControllerPTH10X::gpio_config_info_storage_ = {
+  GPIOInfo{GPIOPin::STAGE2_INPUT, gpiod::line::direction::INPUT},
+  GPIOInfo{GPIOPin::MOTOR_ON, gpiod::line::direction::OUTPUT},
+  GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true},
+};
+
+const std::vector<GPIOInfo> & GPIOControllerPTH10X::GetGPIOConfigInfoStorage()
+{
+  return gpio_config_info_storage_;
+}
+
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/panther_system.cpp b/panther_hardware_interfaces/src/panther_system/panther_system.cpp
index 74d63950..a624c58a 100644
--- a/panther_hardware_interfaces/src/panther_system/panther_system.cpp
+++ b/panther_hardware_interfaces/src/panther_system/panther_system.cpp
@@ -28,6 +28,7 @@
 #include "rclcpp/logging.hpp"
 
 #include "panther_hardware_interfaces/utils.hpp"
+#include "panther_utils/common_utilities.hpp"
 #include "panther_utils/diagnostics.hpp"
 
 namespace panther_hardware_interfaces
@@ -429,12 +430,7 @@ void PantherSystem::ReadDriverStatesUpdateFrequency()
 
 void PantherSystem::ConfigureGPIOController()
 {
-  if (IsPantherVersionAtLeast(1.2)) {
-    gpio_controller_ = std::make_shared<GPIOControllerPTH12X>();
-  } else {
-    gpio_controller_ = std::make_shared<GPIOControllerPTH10X>();
-  }
-
+  gpio_controller_ = GPIOControllerFactory::CreateGPIOController(panther_version_);
   gpio_controller_->Start();
 
   RCLCPP_INFO(logger_, "Successfully configured GPIO controller.");
@@ -463,7 +459,7 @@ void PantherSystem::ConfigureEStop()
     throw std::runtime_error("Failed to configure E-Stop, make sure to setup entities first.");
   }
 
-  if (IsPantherVersionAtLeast(1.2f)) {
+  if (panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2)) {
     e_stop_ = std::make_shared<EStopPTH12X>(
       gpio_controller_, roboteq_error_filter_, motors_controller_, motor_controller_write_mtx_,
       std::bind(&PantherSystem::AreVelocityCommandsNearZero, this));
@@ -660,11 +656,6 @@ bool PantherSystem::AreVelocityCommandsNearZero()
   return true;
 }
 
-bool PantherSystem::IsPantherVersionAtLeast(const float version)
-{
-  return panther_version_ >= version - std::numeric_limits<float>::epsilon();
-}
-
 void PantherSystem::MotorsPowerEnable(const bool enable)
 {
   try {
diff --git a/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_controller.cpp b/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_controller.cpp
index 2e89d879..aa50bf30 100644
--- a/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_controller.cpp
+++ b/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_controller.cpp
@@ -13,8 +13,11 @@
 // limitations under the License.
 
 #include <cstdlib>
+#include <functional>
 
+#include <gmock/gmock.h>
 #include <gtest/gtest.h>
+
 #include <gpiod.hpp>
 
 #include <panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp>
@@ -22,27 +25,31 @@
 using GPIOInfo = panther_hardware_interfaces::GPIOInfo;
 using GPIOPin = panther_hardware_interfaces::GPIOPin;
 
-const std::vector<GPIOInfo> gpio_config_info_storage{
-  GPIOInfo{GPIOPin::WATCHDOG, gpiod::line::direction::OUTPUT},
-  GPIOInfo{GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT},
-  GPIOInfo{GPIOPin::CHRG_DISABLE, gpiod::line::direction::OUTPUT},
-  GPIOInfo{GPIOPin::DRIVER_EN, gpiod::line::direction::OUTPUT},
-  GPIOInfo{GPIOPin::E_STOP_RESET, gpiod::line::direction::INPUT},
-  GPIOInfo{GPIOPin::FAN_SW, gpiod::line::direction::OUTPUT},
-  GPIOInfo{GPIOPin::GPOUT1, gpiod::line::direction::OUTPUT},
-  GPIOInfo{GPIOPin::GPOUT2, gpiod::line::direction::OUTPUT},
-  GPIOInfo{GPIOPin::GPIN1, gpiod::line::direction::INPUT},
-  GPIOInfo{GPIOPin::GPIN2, gpiod::line::direction::INPUT},
-  GPIOInfo{GPIOPin::SHDN_INIT, gpiod::line::direction::INPUT},
-  GPIOInfo{GPIOPin::VDIG_OFF, gpiod::line::direction::OUTPUT},
-  GPIOInfo{GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT},
-  GPIOInfo{GPIOPin::CHRG_SENSE, gpiod::line::direction::INPUT},
-  GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT},
+class MockGPIODriver : public panther_hardware_interfaces::GPIODriverInterface
+{
+public:
+  MOCK_METHOD(
+    void, GPIOMonitorEnable, (const bool use_rt, const unsigned gpio_monit_thread_sched_priority),
+    (override));
+  MOCK_METHOD(
+    void, ConfigureEdgeEventCallback, (const std::function<void(const GPIOInfo &)> & callback),
+    (override));
+  MOCK_METHOD(
+    void, ChangePinDirection, (const GPIOPin pin, const gpiod::line::direction direction),
+    (override));
+  MOCK_METHOD(bool, IsPinAvailable, (const GPIOPin pin), (const, override));
+  MOCK_METHOD(bool, IsPinActive, (const GPIOPin pin), (override));
+  MOCK_METHOD(bool, SetPinValue, (const GPIOPin pin, const bool value), (override));
 };
 
 class GPIOControllerWrapper : public panther_hardware_interfaces::GPIOControllerPTH12X
 {
 public:
+  GPIOControllerWrapper(std::shared_ptr<MockGPIODriver> gpio_driver)
+  : panther_hardware_interfaces::GPIOControllerPTH12X(gpio_driver)
+  {
+  }
+
   void WatchdogEnable() { this->watchdog_->TurnOn(); }
   void WatchdogDisable() { this->watchdog_->TurnOff(); }
   bool IsWatchdogEnabled() { return this->watchdog_->IsWatchdogEnabled(); }
@@ -57,118 +64,129 @@ class TestGPIOController : public ::testing::Test
 protected:
   float GetRobotVersion();
 
+  std::shared_ptr<MockGPIODriver> gpio_driver_;
   std::unique_ptr<GPIOControllerWrapper> gpio_controller_wrapper_;
   static constexpr int watchdog_edges_per_100ms_ = 10;
 };
 
 TestGPIOController::TestGPIOController()
 {
-  if (GetRobotVersion() < 1.2 - std::numeric_limits<float>::epsilon()) {
-    throw std::runtime_error("Tests for this robot versions are not implemented");
-  }
+  gpio_driver_ = std::make_shared<MockGPIODriver>();
 
-  gpio_controller_wrapper_ = std::make_unique<GPIOControllerWrapper>();
+  // Mock methods called during the initialization process
+  ON_CALL(*gpio_driver_, SetPinValue(GPIOPin::VMOT_ON, true)).WillByDefault(testing::Return(true));
+  ON_CALL(*gpio_driver_, SetPinValue(GPIOPin::DRIVER_EN, true))
+    .WillByDefault(testing::Return(true));
+  ON_CALL(*gpio_driver_, IsPinAvailable(GPIOPin::WATCHDOG)).WillByDefault(testing::Return(true));
+
+  gpio_controller_wrapper_ = std::make_unique<GPIOControllerWrapper>(gpio_driver_);
   gpio_controller_wrapper_->Start();
 }
 
-float TestGPIOController::GetRobotVersion()
+struct GPIOTestParam
 {
-  const char * robot_version_env = std::getenv("PANTHER_ROBOT_VERSION");
-
-  if (!robot_version_env) {
-    throw std::runtime_error("Can't read 'PANTHER_ROBOT_VERSION' environment variable");
-  }
-
-  return std::stof(robot_version_env);
-}
+  GPIOPin pin;
+  std::function<bool(GPIOControllerWrapper *, bool)> enable_method;
+  bool is_inverted = false;
+};
 
-TEST_F(TestGPIOController, TestMotorsInit)
+class ParametrizedTestGPIOController : public TestGPIOController,
+                                       public ::testing::WithParamInterface<GPIOTestParam>
 {
-  EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::VMOT_ON));
-  EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::DRIVER_EN));
-}
+};
 
-TEST_F(TestGPIOController, TestWatchdog)
+TEST(TestGPIOControllerInitialization, GPIODriverUninitialized)
 {
-  auto edge_cnt = std::make_shared<int>(0);
+  std::shared_ptr<MockGPIODriver> gpio_driver;
 
-  gpio_controller_wrapper_->RegisterGPIOEventCallback([this, edge_cnt](const auto & state) mutable {
-    if (state.pin == GPIOPin::WATCHDOG) {
-      (*edge_cnt)++;
-    }
-  });
+  EXPECT_THROW(std::make_unique<GPIOControllerWrapper>(gpio_driver), std::runtime_error);
+}
 
-  gpio_controller_wrapper_->WatchdogEnable();
-  ASSERT_TRUE(gpio_controller_wrapper_->IsWatchdogEnabled());
+TEST(TestGPIOControllerInitialization, WatchdogPinNotAvailable)
+{
+  auto gpio_driver = std::make_shared<MockGPIODriver>();
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  gpio_controller_wrapper_->WatchdogDisable();
+  EXPECT_CALL(*gpio_driver, SetPinValue(testing::_, true))
+    .Times(2)
+    .WillRepeatedly(testing::Return(true));
+  ON_CALL(*gpio_driver, IsPinAvailable(GPIOPin::WATCHDOG)).WillByDefault(testing::Return(false));
 
-  ASSERT_FALSE(gpio_controller_wrapper_->IsWatchdogEnabled());
-  EXPECT_EQ(*edge_cnt, watchdog_edges_per_100ms_ + 1);
+  auto gpio_controller_wrapper = std::make_unique<GPIOControllerWrapper>(gpio_driver);
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(50));
-  EXPECT_EQ(*edge_cnt, watchdog_edges_per_100ms_ + 1);
+  EXPECT_THROW(gpio_controller_wrapper->Start(), std::runtime_error);
 }
 
-TEST_F(TestGPIOController, TestPinsAvailability)
+TEST_F(TestGPIOController, TestMotorsInitSuccess)
 {
-  for (const auto & info : gpio_config_info_storage) {
-    EXPECT_TRUE(gpio_controller_wrapper_->IsPinAvailable(info.pin));
-  }
+  ON_CALL(*gpio_driver_, IsPinActive(GPIOPin::VMOT_ON)).WillByDefault(testing::Return(true));
+  ON_CALL(*gpio_driver_, IsPinActive(GPIOPin::DRIVER_EN)).WillByDefault(testing::Return(true));
+
+  EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::VMOT_ON));
+  EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::DRIVER_EN));
 }
 
-TEST_F(TestGPIOController, TestFanEnbale)
+TEST_F(TestGPIOController, TestEStopResetAlreadyDeactivated)
 {
-  gpio_controller_wrapper_->FanEnable(true);
-  EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::FAN_SW));
+  // The E_STOP_RESET pin is already deactivated
+  EXPECT_CALL(*gpio_driver_, IsPinActive(GPIOPin::E_STOP_RESET)).WillOnce(testing::Return(true));
 
-  gpio_controller_wrapper_->FanEnable(false);
-  EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(GPIOPin::FAN_SW));
+  gpio_controller_wrapper_->EStopReset();
 }
 
-TEST_F(TestGPIOController, TestAUXPowerEnbale)
+TEST_P(ParametrizedTestGPIOController, TestGPIOEnableDisable)
 {
-  gpio_controller_wrapper_->AUXPowerEnable(true);
-  EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::AUX_PW_EN));
+  const auto & param = GetParam();
 
-  gpio_controller_wrapper_->AUXPowerEnable(false);
-  EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(GPIOPin::AUX_PW_EN));
-}
+  // Set the enable command based on the pin inversion
+  auto const enable = param.is_inverted ? false : true;
+  auto const disable = !enable;
 
-TEST_F(TestGPIOController, TestChargerEnable)
-{
-  gpio_controller_wrapper_->ChargerEnable(true);
-  EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(GPIOPin::CHRG_DISABLE));
+  // Enable GPIO
+  EXPECT_CALL(*gpio_driver_, SetPinValue(param.pin, enable)).WillOnce(testing::Return(true));
+  EXPECT_CALL(*gpio_driver_, IsPinActive(param.pin)).WillOnce(testing::Return(enable));
 
-  gpio_controller_wrapper_->ChargerEnable(false);
-  EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::CHRG_DISABLE));
-}
+  param.enable_method(gpio_controller_wrapper_.get(), true);
+  EXPECT_EQ(enable, gpio_controller_wrapper_->IsPinActive(param.pin));
 
-TEST_F(TestGPIOController, TestLEDControlEnable)
-{
-  gpio_controller_wrapper_->LEDControlEnable(true);
-  EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::LED_SBC_SEL));
+  // Disable GPIO
+  EXPECT_CALL(*gpio_driver_, SetPinValue(param.pin, disable)).WillOnce(testing::Return(true));
+  EXPECT_CALL(*gpio_driver_, IsPinActive(param.pin)).WillOnce(testing::Return(disable));
 
-  gpio_controller_wrapper_->LEDControlEnable(false);
-  EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(GPIOPin::LED_SBC_SEL));
+  param.enable_method(gpio_controller_wrapper_.get(), false);
+  EXPECT_EQ(disable, gpio_controller_wrapper_->IsPinActive(param.pin));
 }
 
+INSTANTIATE_TEST_SUITE_P(
+  GPIOTests, ParametrizedTestGPIOController,
+  ::testing::Values(
+    GPIOTestParam{GPIOPin::DRIVER_EN, &GPIOControllerWrapper::MotorPowerEnable},
+    GPIOTestParam{GPIOPin::AUX_PW_EN, &GPIOControllerWrapper::AUXPowerEnable},
+    GPIOTestParam{GPIOPin::FAN_SW, &GPIOControllerWrapper::FanEnable},
+    GPIOTestParam{GPIOPin::VDIG_OFF, &GPIOControllerWrapper::DigitalPowerEnable, true},
+    GPIOTestParam{GPIOPin::CHRG_DISABLE, &GPIOControllerWrapper::ChargerEnable, true},
+    GPIOTestParam{GPIOPin::LED_SBC_SEL, &GPIOControllerWrapper::LEDControlEnable}));
+
 TEST_F(TestGPIOController, TestQueryControlInterfaceIOStates)
 {
+  EXPECT_CALL(*gpio_driver_, IsPinActive(testing::_))
+    .Times(7)
+    .WillRepeatedly(testing::Return(true));
+
   std::unordered_map<GPIOPin, bool> io_states =
     gpio_controller_wrapper_->QueryControlInterfaceIOStates();
 
   ASSERT_EQ(io_states.size(), 7);
 
-  for (const auto & [pin, expected_state] : io_states) {
-    bool actual_state = gpio_controller_wrapper_->IsPinActive(pin);
-    EXPECT_EQ(expected_state, actual_state);
+  for (const auto & [pin, state] : io_states) {
+    EXPECT_TRUE(state);
   }
 }
 
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
+
+  auto result = RUN_ALL_TESTS();
+
+  return result;
 }
diff --git a/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_driver.cpp b/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_driver.cpp
index f8f787f2..aede6036 100644
--- a/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_driver.cpp
+++ b/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_driver.cpp
@@ -21,8 +21,8 @@
 #include <utility>
 #include <vector>
 
-#include "gpiod.hpp"
-#include "gtest/gtest.h"
+#include <gtest/gtest.h>
+#include <gpiod.hpp>
 
 #include "panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp"
 #include "panther_utils/test/test_utils.hpp"
@@ -102,15 +102,41 @@ TEST(TestGPIODriverInitialization, WrongPinConfigFail)
 
 TEST_F(TestGPIODriver, SetWrongPinValue)
 {
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::invalid_argument>(
+  auto is_message_thrown = panther_utils::test_utils::IsMessageThrown<std::invalid_argument>(
     [&]() { this->gpio_driver_->SetPinValue(static_cast<GPIOPin>(-1), true); },
-    "Pin not found in GPIO info storage."));
+    "Pin not found in GPIO info storage.");
+
+  EXPECT_TRUE(is_message_thrown);
+}
+
+TEST_F(TestGPIODriver, IsPinAvailableValidPin)
+{
+  auto is_pin_available = this->gpio_driver_->IsPinAvailable(GPIOPin::LED_SBC_SEL);
+
+  EXPECT_TRUE(is_pin_available);
+}
+
+TEST_F(TestGPIODriver, IsPinAvailableInvalidPin)
+{
+  auto is_pin_available = this->gpio_driver_->IsPinAvailable(static_cast<GPIOPin>(-1));
+
+  EXPECT_FALSE(is_pin_available);
+}
+
+TEST_F(TestGPIODriver, IsPinActive)
+{
+  this->gpio_driver_->GPIOMonitorEnable();
+
+  SetAndVerifyPinState(GPIOPin::LED_SBC_SEL);
 }
 
-TEST_F(TestGPIODriver, IsPinAvailable)
+TEST_F(TestGPIODriver, IsPinActiveNoMonitorThread)
 {
-  EXPECT_TRUE(this->gpio_driver_->IsPinAvailable(GPIOPin::LED_SBC_SEL));
-  EXPECT_FALSE(this->gpio_driver_->IsPinAvailable(static_cast<GPIOPin>(-1)));
+  auto is_message_thrown = panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { this->gpio_driver_->IsPinActive(GPIOPin::LED_SBC_SEL); },
+    "GPIO monitor thread is not running!");
+
+  EXPECT_TRUE(is_message_thrown);
 }
 
 TEST_F(TestGPIODriver, GPIOMonitorEnableNoRT)
@@ -138,14 +164,16 @@ TEST_F(TestGPIODriver, GPIOMonitorEnableUseRT)
   SetAndVerifyPinState(GPIOPin::LED_SBC_SEL);
 }
 
-TEST_F(TestGPIODriver, GPIOEventCallbackFailWhenNoMonitorThread)
+TEST_F(TestGPIODriver, GPIOEventCallbackNoMonitorThread)
 {
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  auto is_message_thrown = panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() {
       this->gpio_driver_->ConfigureEdgeEventCallback(
         std::bind(&TestGPIODriver::GPIOEventCallback, this, std::placeholders::_1));
     },
-    "GPIO monitor thread is not running!"));
+    "GPIO monitor thread is not running!");
+
+  EXPECT_TRUE(is_message_thrown);
 }
 
 TEST_F(TestGPIODriver, GPIOEventCallbackShareNewPinState)
@@ -174,9 +202,11 @@ TEST_F(TestGPIODriver, ChangePinDirection)
   this->gpio_driver_->GPIOMonitorEnable();
   this->gpio_driver_->ChangePinDirection(GPIOPin::LED_SBC_SEL, gpiod::line::direction::INPUT);
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::invalid_argument>(
+  auto is_message_thrown = panther_utils::test_utils::IsMessageThrown<std::invalid_argument>(
     [&]() { this->gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, true); },
-    "Cannot set value for INPUT pin."));
+    "Cannot set value for INPUT pin.");
+
+  ASSERT_TRUE(is_message_thrown);
 
   this->gpio_driver_->ChangePinDirection(GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT);
 
@@ -186,5 +216,8 @@ TEST_F(TestGPIODriver, ChangePinDirection)
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
+
+  auto result = RUN_ALL_TESTS();
+
+  return result;
 }
diff --git a/panther_utils/include/panther_utils/common_utilities.hpp b/panther_utils/include/panther_utils/common_utilities.hpp
index a8022559..237f24b5 100644
--- a/panther_utils/include/panther_utils/common_utilities.hpp
+++ b/panther_utils/include/panther_utils/common_utilities.hpp
@@ -15,6 +15,7 @@
 #ifndef PANTHER_UTILS_COMMON_UTILITIES_HPP_
 #define PANTHER_UTILS_COMMON_UTILITIES_HPP_
 
+#include <cmath>
 #include <fstream>
 #include <map>
 #include <sstream>
@@ -95,7 +96,7 @@ std::map<std::string, T> PrefixMapKeys(
  *
  * @throws std::runtime_error if the file fails to open.
  */
-std::fstream OpenFile(const std::string & file_path, const std::ios_base::openmode & mode)
+inline std::fstream OpenFile(const std::string & file_path, const std::ios_base::openmode & mode)
 {
   std::fstream file(file_path, mode);
   if (!file.is_open()) {
@@ -104,6 +105,22 @@ std::fstream OpenFile(const std::string & file_path, const std::ios_base::openmo
   return file;
 }
 
+/**
+ * @brief Checks if required version is satisfied.
+ *
+ * @param version Version to be verified.
+ * @param required_version The required version.
+ * @return true if the required version is satisfied, false otherwise.
+ */
+inline bool MeetsVersionRequirement(const float version, const float required_version)
+{
+  if (std::isnan(version) || std::isnan(required_version)) {
+    return false;
+  }
+
+  return version >= required_version - std::numeric_limits<float>::epsilon();
+}
+
 }  // namespace panther_utils::common_utilities
 
 #endif  // PANTHER_UTILS_COMMON_UTILITIES_HPP_
diff --git a/panther_utils/test/test_common_utilities.cpp b/panther_utils/test/test_common_utilities.cpp
index 0527b69b..031dd043 100644
--- a/panther_utils/test/test_common_utilities.cpp
+++ b/panther_utils/test/test_common_utilities.cpp
@@ -158,6 +158,42 @@ TEST(TestOpenFile, HandleOpenFileThrow)
     { panther_utils::common_utilities::OpenFile(path, std::ios_base::in); }, std::runtime_error);
 }
 
+TEST(TestMeetsVersionRequirement, SameVersion)
+{
+  float version = 1.06;
+
+  auto is_met = panther_utils::common_utilities::MeetsVersionRequirement(version, 1.06);
+
+  EXPECT_TRUE(is_met);
+}
+
+TEST(TestMeetsVersionRequirement, LowerVersion)
+{
+  float version = 1.2;
+
+  auto is_met = panther_utils::common_utilities::MeetsVersionRequirement(version, 1.06);
+
+  EXPECT_TRUE(is_met);
+}
+
+TEST(TestMeetsVersionRequirement, HigherVersion)
+{
+  float version = 1.0;
+
+  auto is_met = panther_utils::common_utilities::MeetsVersionRequirement(version, 1.06);
+
+  EXPECT_FALSE(is_met);
+}
+
+TEST(TestMeetsVersionRequirement, NaNVersionRequired)
+{
+  float version = std::numeric_limits<float>::quiet_NaN();
+
+  auto is_met = panther_utils::common_utilities::MeetsVersionRequirement(version, 1.06);
+
+  EXPECT_FALSE(is_met);
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);

From b1a2bfff9766ccb555f98daec58e10dc2e3f3dec Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Wed, 4 Sep 2024 07:10:56 +0000
Subject: [PATCH 034/100] driver definition

---
 .../robot_driver/lynx_robot_driver.hpp        |  15 +-
 .../robot_driver/panther_robot_driver.hpp     |  15 +-
 .../robot_driver/lynx_robot_driver.cpp        |  23 +-
 .../robot_driver/panther_robot_driver.cpp     |  31 +-
 .../robot_driver/test_lynx_robot_driver.cpp   | 240 +++++-----
 .../test_panther_robot_driver.cpp             | 414 ++++++++++--------
 .../test/utils/mock_driver.hpp                |  69 +++
 7 files changed, 494 insertions(+), 313 deletions(-)
 create mode 100644 panther_hardware_interfaces/test/utils/mock_driver.hpp

diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
index 51211bc6..3bc0b3df 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
@@ -54,8 +54,7 @@ class LynxRobotDriver : public RobotDriver
 {
 public:
   LynxRobotDriver(
-    const std::shared_ptr<Driver> driver, const CANopenSettings & canopen_settings,
-    const DrivetrainSettings & drivetrain_settings,
+    const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings,
     const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000));
 
   ~LynxRobotDriver()
@@ -157,6 +156,15 @@ class LynxRobotDriver : public RobotDriver
    */
   inline void AttemptErrorFlagResetWithZeroSpeed() { SendSpeedCommands(0.0, 0.0, 0.0, 0.0); };
 
+protected:
+  /**
+   * @brief This method defines driver object and adds motor drivers to it. It is virtual to allow
+   * mocking driver in tests.
+   */
+  virtual void DefineDriver();
+
+  std::shared_ptr<Driver> driver_;
+
 private:
   void SetMotorsStates(
     RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
@@ -165,10 +173,9 @@ class LynxRobotDriver : public RobotDriver
 
   bool initialized_ = false;
 
+  CANopenSettings canopen_settings_;
   CANopenManager canopen_manager_;
 
-  std::shared_ptr<Driver> driver_;
-
   RoboteqData data_;
 
   RoboteqVelocityCommandConverter roboteq_vel_cmd_converter_;
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
index 881e464b..2dc16981 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
@@ -55,7 +55,6 @@ class PantherRobotDriver : public RobotDriver
 {
 public:
   PantherRobotDriver(
-    const std::shared_ptr<Driver> front_driver, const std::shared_ptr<Driver> rear_driver,
     const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings,
     const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000));
 
@@ -158,6 +157,16 @@ class PantherRobotDriver : public RobotDriver
    */
   inline void AttemptErrorFlagResetWithZeroSpeed() { SendSpeedCommands(0.0, 0.0, 0.0, 0.0); };
 
+protected:
+  /**
+   * @brief This method defines driver objects and adds motor drivers for them. It is virtual to
+   * allow mocking drivers in tests.
+   */
+  virtual void DefineDrivers();
+
+  std::shared_ptr<Driver> front_driver_;
+  std::shared_ptr<Driver> rear_driver_;
+
 private:
   void SetMotorsStates(
     RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
@@ -166,11 +175,9 @@ class PantherRobotDriver : public RobotDriver
 
   bool initialized_ = false;
 
+  CANopenSettings canopen_settings_;
   CANopenManager canopen_manager_;
 
-  std::shared_ptr<Driver> front_driver_;
-  std::shared_ptr<Driver> rear_driver_;
-
   RoboteqData front_data_;
   RoboteqData rear_data_;
 
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
index fba24edc..786d0156 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
@@ -32,11 +32,10 @@ namespace panther_hardware_interfaces
 {
 
 LynxRobotDriver::LynxRobotDriver(
-  const std::shared_ptr<Driver> driver, const CANopenSettings & canopen_settings,
-  const DrivetrainSettings & drivetrain_settings,
+  const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings,
   const std::chrono::milliseconds activate_wait_time)
-: canopen_manager_(canopen_settings),
-  driver_(std::move(driver)),
+: canopen_settings_(canopen_settings),
+  canopen_manager_(canopen_settings),
   data_(drivetrain_settings),
   roboteq_vel_cmd_converter_(drivetrain_settings),
   pdo_motor_states_timeout_ms_(canopen_settings.pdo_motor_states_timeout_ms),
@@ -53,6 +52,7 @@ void LynxRobotDriver::Initialize()
 
   try {
     canopen_manager_.Initialize();
+    DefineDriver();
     driver_->Boot();
   } catch (const std::runtime_error & e) {
     throw e;
@@ -196,6 +196,21 @@ void LynxRobotDriver::TurnOnSafetyStop()
   }
 }
 
+void LynxRobotDriver::DefineDriver()
+{
+  driver_ = std::make_shared<RoboteqDriver>(
+    canopen_manager_.GetMaster(), canopen_settings_.front_driver_can_id,
+    canopen_settings_.sdo_operation_timeout_ms);
+
+  auto left_motor_driver = std::make_shared<RoboteqMotorDriver>(
+    std::dynamic_pointer_cast<RoboteqDriver>(driver_), LynxMotorChannel::LEFT);
+  auto right_motor_driver = std::make_shared<RoboteqMotorDriver>(
+    std::dynamic_pointer_cast<RoboteqDriver>(driver_), LynxMotorChannel::RIGHT);
+
+  driver_->AddMotorDriver(LynxMotorNames::LEFT, left_motor_driver);
+  driver_->AddMotorDriver(LynxMotorNames::RIGHT, right_motor_driver);
+}
+
 void LynxRobotDriver::SetMotorsStates(
   RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
   const timespec & current_time)
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp
index 5e09ad8e..11a99135 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp
@@ -33,12 +33,10 @@ namespace panther_hardware_interfaces
 {
 
 PantherRobotDriver::PantherRobotDriver(
-  const std::shared_ptr<Driver> front_driver, const std::shared_ptr<Driver> rear_driver,
   const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings,
   const std::chrono::milliseconds activate_wait_time)
-: canopen_manager_(canopen_settings),
-  front_driver_(std::move(front_driver)),
-  rear_driver_(std::move(rear_driver)),
+: canopen_settings_(canopen_settings),
+  canopen_manager_(canopen_settings),
   front_data_(drivetrain_settings),
   rear_data_(drivetrain_settings),
   roboteq_vel_cmd_converter_(drivetrain_settings),
@@ -56,6 +54,7 @@ void PantherRobotDriver::Initialize()
 
   try {
     canopen_manager_.Initialize();
+    DefineDrivers();
     front_driver_->Boot();
     rear_driver_->Boot();
   } catch (const std::runtime_error & e) {
@@ -257,6 +256,30 @@ void PantherRobotDriver::TurnOnSafetyStop()
   }
 }
 
+void PantherRobotDriver::DefineDrivers()
+{
+  front_driver_ = std::make_shared<RoboteqDriver>(
+    canopen_manager_.GetMaster(), canopen_settings_.front_driver_can_id,
+    canopen_settings_.sdo_operation_timeout_ms);
+  rear_driver_ = std::make_shared<RoboteqDriver>(
+    canopen_manager_.GetMaster(), canopen_settings_.rear_driver_can_id,
+    canopen_settings_.sdo_operation_timeout_ms);
+
+  auto fl_motor_driver = std::make_shared<RoboteqMotorDriver>(
+    std::dynamic_pointer_cast<RoboteqDriver>(front_driver_), PantherMotorChannel::LEFT);
+  auto fr_motor_driver = std::make_shared<RoboteqMotorDriver>(
+    std::dynamic_pointer_cast<RoboteqDriver>(front_driver_), PantherMotorChannel::RIGHT);
+  auto rl_motor_driver = std::make_shared<RoboteqMotorDriver>(
+    std::dynamic_pointer_cast<RoboteqDriver>(rear_driver_), PantherMotorChannel::LEFT);
+  auto rr_motor_driver = std::make_shared<RoboteqMotorDriver>(
+    std::dynamic_pointer_cast<RoboteqDriver>(rear_driver_), PantherMotorChannel::RIGHT);
+
+  front_driver_->AddMotorDriver(PantherMotorNames::LEFT, fl_motor_driver);
+  front_driver_->AddMotorDriver(PantherMotorNames::RIGHT, fr_motor_driver);
+  rear_driver_->AddMotorDriver(PantherMotorNames::LEFT, rl_motor_driver);
+  rear_driver_->AddMotorDriver(PantherMotorNames::RIGHT, rr_motor_driver);
+}
+
 void PantherRobotDriver::SetMotorsStates(
   RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
   const timespec & current_time)
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
index ad5c01e6..584a4c00 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
@@ -28,43 +28,58 @@
 #include <panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp>
 
 #include "utils/fake_can_socket.hpp"
+#include "utils/mock_driver.hpp"
 #include "utils/test_constants.hpp"
 
-class MockDriver : public panther_hardware_interfaces::Driver
+class LynxRobotDriverWrapper : public panther_hardware_interfaces::LynxRobotDriver
 {
 public:
-  MOCK_METHOD(std::future<void>, Boot, (), (override));
-  MOCK_METHOD(bool, IsCANError, (), (const, override));
-  MOCK_METHOD(bool, IsHeartbeatTimeout, (), (const, override));
+  LynxRobotDriverWrapper(
+    const panther_hardware_interfaces::CANopenSettings & canopen_settings,
+    const panther_hardware_interfaces::DrivetrainSettings & drivetrain_settings,
+    const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000))
+  : LynxRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time)
+  {
+    mock_left_motor_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+    mock_right_motor_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+
+    mock_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
+    mock_driver_->AddMotorDriver(kLeftMotorDriverName, mock_left_motor_driver_);
+    mock_driver_->AddMotorDriver(kRightMotorDriverName, mock_right_motor_driver_);
+  }
 
-  MOCK_METHOD(panther_hardware_interfaces::DriverState, ReadDriverState, (), (override));
-  MOCK_METHOD(void, ResetScript, (), (override));
-  MOCK_METHOD(void, TurnOnEStop, (), (override));
-  MOCK_METHOD(void, TurnOffEStop, (), (override));
+  void DefineDriver() override { driver_ = mock_driver_; }
 
-  std::shared_ptr<panther_hardware_interfaces::MotorDriver> GetMotorDriver(
-    const std::string & name) override
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>> GetMockDriver()
   {
-    return motor_drivers_.at(name);
+    return mock_driver_;
   }
 
-  void AddMotorDriver(
-    const std::string name,
-    std::shared_ptr<panther_hardware_interfaces::MotorDriver> motor_driver) override
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  GetMockLeftMotorDriver()
   {
-    motor_drivers_.emplace(name, motor_driver);
+    return mock_left_motor_driver_;
   }
 
-private:
-  std::map<std::string, std::shared_ptr<panther_hardware_interfaces::MotorDriver>> motor_drivers_;
-};
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  GetMockRightMotorDriver()
+  {
+    return mock_right_motor_driver_;
+  }
 
-class MockMotorDriver : public panther_hardware_interfaces::MotorDriver
-{
-public:
-  MOCK_METHOD(panther_hardware_interfaces::MotorDriverState, ReadMotorDriverState, (), (override));
-  MOCK_METHOD(void, SendCmdVel, (const std::int32_t cmd), (override));
-  MOCK_METHOD(void, TurnOnSafetyStop, (), (override));
+  static constexpr char kDriverName[] = "default";
+  static constexpr char kLeftMotorDriverName[] = "left";
+  static constexpr char kRightMotorDriverName[] = "right";
+
+private:
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>> mock_driver_;
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+    mock_left_motor_driver_;
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+    mock_right_motor_driver_;
 };
 
 class TestLynxRobotDriverInitialization : public ::testing::Test
@@ -76,91 +91,77 @@ class TestLynxRobotDriverInitialization : public ::testing::Test
       panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
     can_socket_->Initialize();
 
-    left_motor_driver_ = std::make_shared<::testing::NiceMock<MockMotorDriver>>();
-    right_motor_driver_ = std::make_shared<::testing::NiceMock<MockMotorDriver>>();
-
-    driver_mock_ = std::make_shared<::testing::NiceMock<MockDriver>>();
-    driver_mock_->AddMotorDriver(kLeftMotorDriverName, left_motor_driver_);
-    driver_mock_->AddMotorDriver(kRightMotorDriverName, right_motor_driver_);
-
-    robot_driver_ = std::make_unique<panther_hardware_interfaces::LynxRobotDriver>(
-      driver_mock_, panther_hardware_interfaces_test::kCANopenSettings,
+    robot_driver_ = std::make_unique<LynxRobotDriverWrapper>(
+      panther_hardware_interfaces_test::kCANopenSettings,
       panther_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10));
   }
 
   ~TestLynxRobotDriverInitialization() {}
 
 protected:
-  static constexpr char kDriverName[] = "default";
-  static constexpr char kLeftMotorDriverName[] = "left";
-  static constexpr char kRightMotorDriverName[] = "right";
-
-  std::shared_ptr<::testing::NiceMock<MockDriver>> driver_mock_;
-  std::shared_ptr<::testing::NiceMock<MockMotorDriver>> left_motor_driver_;
-  std::shared_ptr<::testing::NiceMock<MockMotorDriver>> right_motor_driver_;
   std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
-  std::unique_ptr<panther_hardware_interfaces::LynxRobotDriver> robot_driver_;
+  std::unique_ptr<LynxRobotDriverWrapper> robot_driver_;
+};
+
+class TestLynxRobotDriver : public TestLynxRobotDriverInitialization
+{
+public:
+  TestLynxRobotDriver()
+  {
+    robot_driver_->Initialize();
+    robot_driver_->Activate();
+  }
+
+  ~TestLynxRobotDriver() { robot_driver_->Deinitialize(); }
 };
 
 TEST_F(TestLynxRobotDriverInitialization, Initialize)
 {
-  EXPECT_CALL(*driver_mock_, Boot()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockDriver(), Boot()).Times(1);
 
-  ASSERT_NO_THROW(robot_driver_->Initialize());
+  EXPECT_NO_THROW(robot_driver_->Initialize());
   ASSERT_NO_THROW(robot_driver_->Deinitialize());
 
-  EXPECT_CALL(*driver_mock_, Boot()).Times(1);
   // Check if deinitialization worked correctly - initialize once again
-  ASSERT_NO_THROW(robot_driver_->Initialize());
-  ASSERT_NO_THROW(robot_driver_->Deinitialize());
+  EXPECT_CALL(*robot_driver_->GetMockDriver(), Boot()).Times(1);
+  EXPECT_NO_THROW(robot_driver_->Initialize());
 }
 
 TEST_F(TestLynxRobotDriverInitialization, Activate)
 {
-  EXPECT_CALL(*driver_mock_, ResetScript()).Times(1);
-  EXPECT_CALL(*left_motor_driver_, SendCmdVel(::testing::Eq(0))).Times(1);
-  EXPECT_CALL(*right_motor_driver_, SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockDriver(), ResetScript()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockLeftMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRightMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
 
   ASSERT_NO_THROW(robot_driver_->Initialize());
-  ASSERT_NO_THROW(robot_driver_->Activate());
+  EXPECT_NO_THROW(robot_driver_->Activate());
 }
 
-class TestLynxRobotDriver : public TestLynxRobotDriverInitialization
-{
-public:
-  TestLynxRobotDriver()
-  {
-    robot_driver_->Initialize();
-    robot_driver_->Activate();
-  }
-
-  ~TestLynxRobotDriver() { robot_driver_->Deinitialize(); }
-};
-
 TEST_F(TestLynxRobotDriver, UpdateCommunicationState)
 {
-  EXPECT_CALL(*driver_mock_, IsCANError()).Times(1);
-  EXPECT_CALL(*driver_mock_, IsHeartbeatTimeout()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockDriver(), IsCANError()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockDriver(), IsHeartbeatTimeout()).Times(1);
 
-  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
+  EXPECT_NO_THROW(robot_driver_->UpdateCommunicationState());
 }
 
 TEST_F(TestLynxRobotDriver, UpdateCommunicationStateCANErorr)
 {
-  EXPECT_CALL(*driver_mock_, IsCANError()).WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->GetMockDriver(), IsCANError()).WillOnce(::testing::Return(true));
 
   ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
 
-  EXPECT_TRUE(robot_driver_->GetData(kDriverName).IsCANError());
+  EXPECT_TRUE(robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsCANError());
 }
 
 TEST_F(TestLynxRobotDriver, UpdateCommunicationStateHeartbeatTimeout)
 {
-  EXPECT_CALL(*driver_mock_, IsHeartbeatTimeout()).WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->GetMockDriver(), IsHeartbeatTimeout())
+    .WillOnce(::testing::Return(true));
 
   ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
 
-  EXPECT_TRUE(robot_driver_->GetData(kDriverName).IsHeartbeatTimeout());
+  EXPECT_TRUE(robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsHeartbeatTimeout());
 }
 
 TEST_F(TestLynxRobotDriver, UpdateMotorsState)
@@ -177,26 +178,27 @@ TEST_F(TestLynxRobotDriver, UpdateMotorsState)
   const std::int32_t r_vel = 202;
   const std::int32_t r_current = 203;
 
-  ON_CALL(*left_motor_driver_, ReadMotorDriverState())
+  ON_CALL(*robot_driver_->GetMockLeftMotorDriver(), ReadMotorDriverState())
     .WillByDefault(::testing::Return(
       panther_hardware_interfaces::MotorDriverState({l_pos, l_vel, l_current, {0, 0}, {0, 0}})));
-  ON_CALL(*right_motor_driver_, ReadMotorDriverState())
+  ON_CALL(*robot_driver_->GetMockRightMotorDriver(), ReadMotorDriverState())
     .WillByDefault(::testing::Return(
       panther_hardware_interfaces::MotorDriverState({r_pos, r_vel, r_current, {0, 0}, {0, 0}})));
 
   robot_driver_->UpdateMotorsState();
 
-  const auto & left = robot_driver_->GetData(kDriverName).GetMotorState(LynxMotorChannel::LEFT);
-  const auto & right =
-    robot_driver_->GetData(kDriverName).GetMotorState(LynxMotorChannel::RIGHT);
+  const auto & left_data = robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName)
+                             .GetMotorState(LynxMotorChannel::LEFT);
+  const auto & right_data = robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName)
+                              .GetMotorState(LynxMotorChannel::RIGHT);
 
-  EXPECT_FLOAT_EQ(left.GetPosition(), l_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(left.GetVelocity(), l_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(left.GetTorque(), l_current * kRbtqCurrentFbToNewtonMeters);
+  EXPECT_FLOAT_EQ(left_data.GetPosition(), l_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(left_data.GetVelocity(), l_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(left_data.GetTorque(), l_current * kRbtqCurrentFbToNewtonMeters);
 
-  EXPECT_FLOAT_EQ(right.GetPosition(), r_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(right.GetVelocity(), r_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(right.GetTorque(), r_current * kRbtqCurrentFbToNewtonMeters);
+  EXPECT_FLOAT_EQ(right_data.GetPosition(), r_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(right_data.GetVelocity(), r_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(right_data.GetTorque(), r_current * kRbtqCurrentFbToNewtonMeters);
 }
 
 TEST_F(TestLynxRobotDriver, UpdateMotorsStateTimestamps)
@@ -208,9 +210,9 @@ TEST_F(TestLynxRobotDriver, UpdateMotorsStateTimestamps)
     return state;
   };
 
-  ON_CALL(*left_motor_driver_, ReadMotorDriverState())
+  ON_CALL(*robot_driver_->GetMockLeftMotorDriver(), ReadMotorDriverState())
     .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
-  ON_CALL(*right_motor_driver_, ReadMotorDriverState())
+  ON_CALL(*robot_driver_->GetMockRightMotorDriver(), ReadMotorDriverState())
     .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
 
   robot_driver_->UpdateMotorsState();
@@ -222,7 +224,8 @@ TEST_F(TestLynxRobotDriver, UpdateMotorsStateTimestamps)
 
   robot_driver_->UpdateMotorsState();
 
-  EXPECT_FALSE(robot_driver_->GetData(kDriverName).IsMotorStatesDataTimedOut());
+  EXPECT_FALSE(
+    robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsMotorStatesDataTimedOut());
 }
 
 TEST_F(TestLynxRobotDriver, UpdateMotorsStateTimeout)
@@ -232,8 +235,10 @@ TEST_F(TestLynxRobotDriver, UpdateMotorsStateTimeout)
 
   panther_hardware_interfaces::MotorDriverState state = {0, 0, 0, current_time, current_time};
 
-  ON_CALL(*left_motor_driver_, ReadMotorDriverState()).WillByDefault(::testing::Return(state));
-  ON_CALL(*right_motor_driver_, ReadMotorDriverState()).WillByDefault(::testing::Return(state));
+  ON_CALL(*robot_driver_->GetMockLeftMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Return(state));
+  ON_CALL(*robot_driver_->GetMockRightMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Return(state));
 
   // sleep for pdo_motor_states_timeout_ms + 10ms
   std::this_thread::sleep_for(
@@ -242,8 +247,9 @@ TEST_F(TestLynxRobotDriver, UpdateMotorsStateTimeout)
 
   robot_driver_->UpdateMotorsState();
 
-  EXPECT_TRUE(robot_driver_->GetData(kDriverName).IsMotorStatesDataTimedOut());
-  EXPECT_TRUE(robot_driver_->GetData(kDriverName).IsError());
+  EXPECT_TRUE(
+    robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsMotorStatesDataTimedOut());
+  EXPECT_TRUE(robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsError());
 }
 
 TEST_F(TestLynxRobotDriver, UpdateDriverState)
@@ -261,7 +267,7 @@ TEST_F(TestLynxRobotDriver, UpdateDriverState)
   const std::uint8_t runtime_error_loop_error = static_cast<std::uint8_t>(0b100);
   const std::uint8_t runtime_error_safety_stop_active = static_cast<std::uint8_t>(0b1000);
 
-  ON_CALL(*driver_mock_, ReadDriverState())
+  ON_CALL(*robot_driver_->GetMockDriver(), ReadDriverState())
     .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
       {fault_flag_overheat,
        script_flag_encoder_disconnected,
@@ -277,8 +283,9 @@ TEST_F(TestLynxRobotDriver, UpdateDriverState)
 
   robot_driver_->UpdateDriversState();
 
-  const auto & data = robot_driver_->GetData(kDriverName);
-  const auto & driver_state = robot_driver_->GetData(kDriverName).GetDriverState();
+  const auto & data = robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName);
+  const auto & driver_state =
+    robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).GetDriverState();
 
   EXPECT_EQ(static_cast<std::int16_t>(driver_state.GetTemperature()), f_temp);
   EXPECT_EQ(static_cast<std::int16_t>(driver_state.GetHeatsinkTemperature()), f_heatsink_temp);
@@ -302,7 +309,7 @@ TEST_F(TestLynxRobotDriver, UpdateDriverStateTimestamps)
     return state;
   };
 
-  ON_CALL(*driver_mock_, ReadDriverState())
+  ON_CALL(*robot_driver_->GetMockDriver(), ReadDriverState())
     .WillByDefault(::testing::Invoke(read_driver_state_method));
 
   robot_driver_->UpdateDriversState();
@@ -314,7 +321,8 @@ TEST_F(TestLynxRobotDriver, UpdateDriverStateTimestamps)
 
   robot_driver_->UpdateDriversState();
 
-  EXPECT_FALSE(robot_driver_->GetData(kDriverName).IsDriverStateDataTimedOut());
+  EXPECT_FALSE(
+    robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsDriverStateDataTimedOut());
 }
 
 TEST_F(TestLynxRobotDriver, UpdateDriverStateTimeout)
@@ -325,7 +333,8 @@ TEST_F(TestLynxRobotDriver, UpdateDriverStateTimeout)
   panther_hardware_interfaces::DriverState state = {
     0, 0, 0, 0, 0, 0, 0, 0, 0, current_time, current_time};
 
-  ON_CALL(*driver_mock_, ReadDriverState()).WillByDefault(::testing::Return(state));
+  ON_CALL(*robot_driver_->GetMockDriver(), ReadDriverState())
+    .WillByDefault(::testing::Return(state));
 
   // sleep for pdo_driver_state_timeout_ms + 10ms
   std::this_thread::sleep_for(
@@ -334,8 +343,9 @@ TEST_F(TestLynxRobotDriver, UpdateDriverStateTimeout)
 
   robot_driver_->UpdateDriversState();
 
-  EXPECT_TRUE(robot_driver_->GetData(kDriverName).IsDriverStateDataTimedOut());
-  EXPECT_TRUE(robot_driver_->GetData(kDriverName).IsError());
+  EXPECT_TRUE(
+    robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsDriverStateDataTimedOut());
+  EXPECT_TRUE(robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsError());
 }
 
 TEST_F(TestLynxRobotDriver, SendSpeedCommands)
@@ -346,60 +356,58 @@ TEST_F(TestLynxRobotDriver, SendSpeedCommands)
   const float r_v = 0.2;
 
   EXPECT_CALL(
-    *left_motor_driver_,
+    *robot_driver_->GetMockLeftMotorDriver(),
     SendCmdVel(::testing::Eq(static_cast<std::int32_t>(l_v * kRadPerSecToRbtqCmd))))
     .Times(1);
   EXPECT_CALL(
-    *right_motor_driver_,
+    *robot_driver_->GetMockRightMotorDriver(),
     SendCmdVel(::testing::Eq(static_cast<std::int32_t>(r_v * kRadPerSecToRbtqCmd))))
     .Times(1);
 
-  ASSERT_NO_THROW(robot_driver_->SendSpeedCommands(l_v, r_v, 0, 0));
+  EXPECT_NO_THROW(robot_driver_->SendSpeedCommands(l_v, r_v, 0, 0));
 }
 
 TEST_F(TestLynxRobotDriver, TurnOnEStop)
 {
-  EXPECT_CALL(*driver_mock_, TurnOnEStop()).Times(1);
-
-  ASSERT_NO_THROW(robot_driver_->TurnOnEStop());
+  EXPECT_CALL(*robot_driver_->GetMockDriver(), TurnOnEStop()).Times(1);
+  EXPECT_NO_THROW(robot_driver_->TurnOnEStop());
 }
 
 TEST_F(TestLynxRobotDriver, TurnOffEStop)
 {
-  EXPECT_CALL(*driver_mock_, TurnOffEStop()).Times(1);
-
-  ASSERT_NO_THROW(robot_driver_->TurnOffEStop());
+  EXPECT_CALL(*robot_driver_->GetMockDriver(), TurnOffEStop()).Times(1);
+  EXPECT_NO_THROW(robot_driver_->TurnOffEStop());
 }
 
 TEST_F(TestLynxRobotDriver, TurnOnEStopError)
 {
-  EXPECT_CALL(*driver_mock_, TurnOnEStop()).WillOnce(::testing::Throw(std::runtime_error("")));
-
-  ASSERT_THROW(robot_driver_->TurnOnEStop(), std::runtime_error);
+  EXPECT_CALL(*robot_driver_->GetMockDriver(), TurnOnEStop())
+    .WillOnce(::testing::Throw(std::runtime_error("")));
+  EXPECT_THROW(robot_driver_->TurnOnEStop(), std::runtime_error);
 }
 
 TEST_F(TestLynxRobotDriver, TurnOffEStopError)
 {
-  EXPECT_CALL(*driver_mock_, TurnOffEStop()).WillOnce(::testing::Throw(std::runtime_error("")));
-
-  ASSERT_THROW(robot_driver_->TurnOffEStop(), std::runtime_error);
+  EXPECT_CALL(*robot_driver_->GetMockDriver(), TurnOffEStop())
+    .WillOnce(::testing::Throw(std::runtime_error("")));
+  EXPECT_THROW(robot_driver_->TurnOffEStop(), std::runtime_error);
 }
 
 TEST_F(TestLynxRobotDriver, SafetyStop)
 {
-  EXPECT_CALL(*left_motor_driver_, TurnOnSafetyStop()).Times(1);
-  EXPECT_CALL(*right_motor_driver_, TurnOnSafetyStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockLeftMotorDriver(), TurnOnSafetyStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRightMotorDriver(), TurnOnSafetyStop()).Times(1);
 
-  ASSERT_NO_THROW(robot_driver_->TurnOnSafetyStop());
+  EXPECT_NO_THROW(robot_driver_->TurnOnSafetyStop());
 }
 
 TEST_F(TestLynxRobotDriver, SafetyStopError)
 {
-  EXPECT_CALL(*left_motor_driver_, TurnOnSafetyStop())
+  EXPECT_CALL(*robot_driver_->GetMockLeftMotorDriver(), TurnOnSafetyStop())
     .WillOnce(::testing::Throw(std::runtime_error("")));
-  EXPECT_CALL(*right_motor_driver_, TurnOnSafetyStop()).Times(0);
+  EXPECT_CALL(*robot_driver_->GetMockRightMotorDriver(), TurnOnSafetyStop()).Times(0);
 
-  ASSERT_THROW(robot_driver_->TurnOnSafetyStop(), std::runtime_error);
+  EXPECT_THROW(robot_driver_->TurnOnSafetyStop(), std::runtime_error);
 }
 
 int main(int argc, char ** argv)
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
index cc0da7d0..1ef15d28 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
@@ -28,43 +28,98 @@
 #include <panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp>
 
 #include "utils/fake_can_socket.hpp"
+#include "utils/mock_driver.hpp"
 #include "utils/test_constants.hpp"
 
-class MockDriver : public panther_hardware_interfaces::Driver
+class PantherRobotDriverWrapper : public panther_hardware_interfaces::PantherRobotDriver
 {
 public:
-  MOCK_METHOD(std::future<void>, Boot, (), (override));
-  MOCK_METHOD(bool, IsCANError, (), (const, override));
-  MOCK_METHOD(bool, IsHeartbeatTimeout, (), (const, override));
+  PantherRobotDriverWrapper(
+    const panther_hardware_interfaces::CANopenSettings & canopen_settings,
+    const panther_hardware_interfaces::DrivetrainSettings & drivetrain_settings,
+    const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000))
+  : PantherRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time)
+  {
+    mock_fl_motor_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+    mock_fr_motor_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+    mock_rl_motor_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+    mock_rr_motor_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+
+    mock_front_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
+    mock_front_driver_->AddMotorDriver(kLeftMotorDriverName, mock_fl_motor_driver_);
+    mock_front_driver_->AddMotorDriver(kRightMotorDriverName, mock_fr_motor_driver_);
+
+    mock_rear_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
+    mock_rear_driver_->AddMotorDriver(kLeftMotorDriverName, mock_rl_motor_driver_);
+    mock_rear_driver_->AddMotorDriver(kRightMotorDriverName, mock_rr_motor_driver_);
+  }
 
-  MOCK_METHOD(panther_hardware_interfaces::DriverState, ReadDriverState, (), (override));
-  MOCK_METHOD(void, ResetScript, (), (override));
-  MOCK_METHOD(void, TurnOnEStop, (), (override));
-  MOCK_METHOD(void, TurnOffEStop, (), (override));
+  void DefineDrivers() override
+  {
+    front_driver_ = mock_front_driver_;
+    rear_driver_ = mock_rear_driver_;
+  }
 
-  std::shared_ptr<panther_hardware_interfaces::MotorDriver> GetMotorDriver(
-    const std::string & name) override
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
+  GetMockFrontDriver()
   {
-    return motor_drivers_.at(name);
+    return mock_front_driver_;
   }
 
-  void AddMotorDriver(
-    const std::string name,
-    std::shared_ptr<panther_hardware_interfaces::MotorDriver> motor_driver) override
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
+  GetMockRearDriver()
   {
-    motor_drivers_.emplace(name, motor_driver);
+    return mock_rear_driver_;
   }
 
-private:
-  std::map<std::string, std::shared_ptr<panther_hardware_interfaces::MotorDriver>> motor_drivers_;
-};
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  GetMockFLMotorDriver()
+  {
+    return mock_fl_motor_driver_;
+  }
 
-class MockMotorDriver : public panther_hardware_interfaces::MotorDriver
-{
-public:
-  MOCK_METHOD(panther_hardware_interfaces::MotorDriverState, ReadMotorDriverState, (), (override));
-  MOCK_METHOD(void, SendCmdVel, (const std::int32_t cmd), (override));
-  MOCK_METHOD(void, TurnOnSafetyStop, (), (override));
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  GetMockFRMotorDriver()
+  {
+    return mock_fr_motor_driver_;
+  }
+
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  GetMockRLMotorDriver()
+  {
+    return mock_rl_motor_driver_;
+  }
+
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  GetMockRRMotorDriver()
+  {
+    return mock_rr_motor_driver_;
+  }
+
+  static constexpr char kFrontDriverName[] = "front";
+  static constexpr char kRearDriverName[] = "rear";
+  static constexpr char kLeftMotorDriverName[] = "left";
+  static constexpr char kRightMotorDriverName[] = "right";
+
+private:
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
+    mock_front_driver_;
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
+    mock_rear_driver_;
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+    mock_fl_motor_driver_;
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+    mock_fr_motor_driver_;
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+    mock_rl_motor_driver_;
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+    mock_rr_motor_driver_;
 };
 
 class TestPantherRobotDriverInitialization : public ::testing::Test
@@ -76,112 +131,91 @@ class TestPantherRobotDriverInitialization : public ::testing::Test
       panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
     can_socket_->Initialize();
 
-    fl_motor_driver_ = std::make_shared<::testing::NiceMock<MockMotorDriver>>();
-    fr_motor_driver_ = std::make_shared<::testing::NiceMock<MockMotorDriver>>();
-    rl_motor_driver_ = std::make_shared<::testing::NiceMock<MockMotorDriver>>();
-    rr_motor_driver_ = std::make_shared<::testing::NiceMock<MockMotorDriver>>();
-
-    front_driver_mock_ = std::make_shared<::testing::NiceMock<MockDriver>>();
-    front_driver_mock_->AddMotorDriver(kLeftMotorDriverName, fl_motor_driver_);
-    front_driver_mock_->AddMotorDriver(kRightMotorDriverName, fr_motor_driver_);
-
-    rear_driver_mock_ = std::make_shared<::testing::NiceMock<MockDriver>>();
-    rear_driver_mock_->AddMotorDriver(kLeftMotorDriverName, rl_motor_driver_);
-    rear_driver_mock_->AddMotorDriver(kRightMotorDriverName, rr_motor_driver_);
-
-    robot_driver_ = std::make_unique<panther_hardware_interfaces::PantherRobotDriver>(
-      front_driver_mock_, rear_driver_mock_, panther_hardware_interfaces_test::kCANopenSettings,
+    robot_driver_ = std::make_unique<PantherRobotDriverWrapper>(
+      panther_hardware_interfaces_test::kCANopenSettings,
       panther_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10));
   }
 
   ~TestPantherRobotDriverInitialization() {}
 
 protected:
-  static constexpr char kFrontDriverName[] = "front";
-  static constexpr char kRearDriverName[] = "rear";
-  static constexpr char kLeftMotorDriverName[] = "left";
-  static constexpr char kRightMotorDriverName[] = "right";
-
-  std::shared_ptr<::testing::NiceMock<MockDriver>> front_driver_mock_;
-  std::shared_ptr<::testing::NiceMock<MockDriver>> rear_driver_mock_;
-  std::shared_ptr<::testing::NiceMock<MockMotorDriver>> fl_motor_driver_;
-  std::shared_ptr<::testing::NiceMock<MockMotorDriver>> fr_motor_driver_;
-  std::shared_ptr<::testing::NiceMock<MockMotorDriver>> rl_motor_driver_;
-  std::shared_ptr<::testing::NiceMock<MockMotorDriver>> rr_motor_driver_;
   std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
-  std::unique_ptr<panther_hardware_interfaces::PantherRobotDriver> robot_driver_;
+  std::unique_ptr<PantherRobotDriverWrapper> robot_driver_;
+};
+
+class TestPantherRobotDriver : public TestPantherRobotDriverInitialization
+{
+public:
+  TestPantherRobotDriver()
+  {
+    robot_driver_->Initialize();
+    robot_driver_->Activate();
+  }
+
+  ~TestPantherRobotDriver() { robot_driver_->Deinitialize(); }
 };
 
 TEST_F(TestPantherRobotDriverInitialization, Initialize)
 {
-  EXPECT_CALL(*front_driver_mock_, Boot()).Times(1);
-  EXPECT_CALL(*rear_driver_mock_, Boot()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), Boot()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), Boot()).Times(1);
 
-  ASSERT_NO_THROW(robot_driver_->Initialize());
+  EXPECT_NO_THROW(robot_driver_->Initialize());
   ASSERT_NO_THROW(robot_driver_->Deinitialize());
 
-  EXPECT_CALL(*front_driver_mock_, Boot()).Times(1);
-  EXPECT_CALL(*rear_driver_mock_, Boot()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), Boot()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), Boot()).Times(1);
   // Check if deinitialization worked correctly - initialize once again
-  ASSERT_NO_THROW(robot_driver_->Initialize());
-  ASSERT_NO_THROW(robot_driver_->Deinitialize());
+  EXPECT_NO_THROW(robot_driver_->Initialize());
 }
 
 TEST_F(TestPantherRobotDriverInitialization, Activate)
 {
-  EXPECT_CALL(*front_driver_mock_, ResetScript()).Times(1);
-  EXPECT_CALL(*rear_driver_mock_, ResetScript()).Times(1);
-  EXPECT_CALL(*fl_motor_driver_, SendCmdVel(::testing::Eq(0))).Times(1);
-  EXPECT_CALL(*fr_motor_driver_, SendCmdVel(::testing::Eq(0))).Times(1);
-  EXPECT_CALL(*rl_motor_driver_, SendCmdVel(::testing::Eq(0))).Times(1);
-  EXPECT_CALL(*rr_motor_driver_, SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), ResetScript()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), ResetScript()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFLMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFRMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRLMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRRMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
 
   ASSERT_NO_THROW(robot_driver_->Initialize());
   ASSERT_NO_THROW(robot_driver_->Activate());
 }
 
-class TestPantherRobotDriver : public TestPantherRobotDriverInitialization
-{
-public:
-  TestPantherRobotDriver()
-  {
-    robot_driver_->Initialize();
-    robot_driver_->Activate();
-  }
-
-  ~TestPantherRobotDriver() { robot_driver_->Deinitialize(); }
-};
-
 TEST_F(TestPantherRobotDriver, UpdateCommunicationState)
 {
-  EXPECT_CALL(*front_driver_mock_, IsCANError()).Times(1);
-  EXPECT_CALL(*front_driver_mock_, IsHeartbeatTimeout()).Times(1);
-  EXPECT_CALL(*rear_driver_mock_, IsCANError()).Times(1);
-  EXPECT_CALL(*rear_driver_mock_, IsHeartbeatTimeout()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsCANError()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsHeartbeatTimeout()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsCANError()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsHeartbeatTimeout()).Times(1);
 
   ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
 }
 
 TEST_F(TestPantherRobotDriver, UpdateCommunicationStateCANErorr)
 {
-  EXPECT_CALL(*front_driver_mock_, IsCANError()).WillOnce(::testing::Return(true));
-  EXPECT_CALL(*rear_driver_mock_, IsCANError()).WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsCANError()).WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsCANError()).WillOnce(::testing::Return(true));
 
   ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
 
-  EXPECT_TRUE(robot_driver_->GetData(kFrontDriverName).IsCANError());
-  EXPECT_TRUE(robot_driver_->GetData(kRearDriverName).IsCANError());
+  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName).IsCANError());
+  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsCANError());
 }
 
 TEST_F(TestPantherRobotDriver, UpdateCommunicationStateHeartbeatTimeout)
 {
-  EXPECT_CALL(*front_driver_mock_, IsHeartbeatTimeout()).WillOnce(::testing::Return(true));
-  EXPECT_CALL(*rear_driver_mock_, IsHeartbeatTimeout()).WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsHeartbeatTimeout())
+    .WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsHeartbeatTimeout())
+    .WillOnce(::testing::Return(true));
 
   ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
 
-  EXPECT_TRUE(robot_driver_->GetData(kFrontDriverName).IsHeartbeatTimeout());
-  EXPECT_TRUE(robot_driver_->GetData(kRearDriverName).IsHeartbeatTimeout());
+  EXPECT_TRUE(
+    robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName).IsHeartbeatTimeout());
+  EXPECT_TRUE(
+    robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsHeartbeatTimeout());
 }
 
 TEST_F(TestPantherRobotDriver, UpdateMotorsState)
@@ -204,45 +238,45 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsState)
   const std::int32_t rr_vel = 402;
   const std::int32_t rr_current = 403;
 
-  ON_CALL(*fl_motor_driver_, ReadMotorDriverState())
+  ON_CALL(*robot_driver_->GetMockFLMotorDriver(), ReadMotorDriverState())
     .WillByDefault(::testing::Return(
       panther_hardware_interfaces::MotorDriverState({fl_pos, fl_vel, fl_current, {0, 0}, {0, 0}})));
-  ON_CALL(*fr_motor_driver_, ReadMotorDriverState())
+  ON_CALL(*robot_driver_->GetMockFRMotorDriver(), ReadMotorDriverState())
     .WillByDefault(::testing::Return(
       panther_hardware_interfaces::MotorDriverState({fr_pos, fr_vel, fr_current, {0, 0}, {0, 0}})));
-  ON_CALL(*rl_motor_driver_, ReadMotorDriverState())
+  ON_CALL(*robot_driver_->GetMockRLMotorDriver(), ReadMotorDriverState())
     .WillByDefault(::testing::Return(
       panther_hardware_interfaces::MotorDriverState({rl_pos, rl_vel, rl_current, {0, 0}, {0, 0}})));
-  ON_CALL(*rr_motor_driver_, ReadMotorDriverState())
+  ON_CALL(*robot_driver_->GetMockRRMotorDriver(), ReadMotorDriverState())
     .WillByDefault(::testing::Return(
       panther_hardware_interfaces::MotorDriverState({rr_pos, rr_vel, rr_current, {0, 0}, {0, 0}})));
 
   robot_driver_->UpdateMotorsState();
 
-  const auto & fl =
-    robot_driver_->GetData(kFrontDriverName).GetMotorState(PantherMotorChannel::LEFT);
-  const auto & fr =
-    robot_driver_->GetData(kFrontDriverName).GetMotorState(PantherMotorChannel::RIGHT);
-  const auto & rl =
-    robot_driver_->GetData(kRearDriverName).GetMotorState(PantherMotorChannel::LEFT);
-  const auto & rr =
-    robot_driver_->GetData(kRearDriverName).GetMotorState(PantherMotorChannel::RIGHT);
-
-  EXPECT_FLOAT_EQ(fl.GetPosition(), fl_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(fl.GetVelocity(), fl_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(fl.GetTorque(), fl_current * kRbtqCurrentFbToNewtonMeters);
-
-  EXPECT_FLOAT_EQ(fr.GetPosition(), fr_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(fr.GetVelocity(), fr_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(fr.GetTorque(), fr_current * kRbtqCurrentFbToNewtonMeters);
-
-  EXPECT_FLOAT_EQ(rl.GetPosition(), rl_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(rl.GetVelocity(), rl_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(rl.GetTorque(), rl_current * kRbtqCurrentFbToNewtonMeters);
-
-  EXPECT_FLOAT_EQ(rr.GetPosition(), rr_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(rr.GetVelocity(), rr_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(rr.GetTorque(), rr_current * kRbtqCurrentFbToNewtonMeters);
+  const auto & fl_data = robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName)
+                           .GetMotorState(PantherMotorChannel::LEFT);
+  const auto & fr_data = robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName)
+                           .GetMotorState(PantherMotorChannel::RIGHT);
+  const auto & rl_data = robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName)
+                           .GetMotorState(PantherMotorChannel::LEFT);
+  const auto & rr_data = robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName)
+                           .GetMotorState(PantherMotorChannel::RIGHT);
+
+  EXPECT_FLOAT_EQ(fl_data.GetPosition(), fl_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(fl_data.GetVelocity(), fl_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(fl_data.GetTorque(), fl_current * kRbtqCurrentFbToNewtonMeters);
+
+  EXPECT_FLOAT_EQ(fr_data.GetPosition(), fr_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(fr_data.GetVelocity(), fr_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(fr_data.GetTorque(), fr_current * kRbtqCurrentFbToNewtonMeters);
+
+  EXPECT_FLOAT_EQ(rl_data.GetPosition(), rl_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(rl_data.GetVelocity(), rl_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(rl_data.GetTorque(), rl_current * kRbtqCurrentFbToNewtonMeters);
+
+  EXPECT_FLOAT_EQ(rr_data.GetPosition(), rr_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(rr_data.GetVelocity(), rr_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(rr_data.GetTorque(), rr_current * kRbtqCurrentFbToNewtonMeters);
 }
 
 TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimestamps)
@@ -254,13 +288,13 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimestamps)
     return state;
   };
 
-  ON_CALL(*fl_motor_driver_, ReadMotorDriverState())
+  ON_CALL(*robot_driver_->GetMockFLMotorDriver(), ReadMotorDriverState())
     .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
-  ON_CALL(*fr_motor_driver_, ReadMotorDriverState())
+  ON_CALL(*robot_driver_->GetMockFRMotorDriver(), ReadMotorDriverState())
     .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
-  ON_CALL(*rl_motor_driver_, ReadMotorDriverState())
+  ON_CALL(*robot_driver_->GetMockRLMotorDriver(), ReadMotorDriverState())
     .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
-  ON_CALL(*rr_motor_driver_, ReadMotorDriverState())
+  ON_CALL(*robot_driver_->GetMockRRMotorDriver(), ReadMotorDriverState())
     .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
 
   robot_driver_->UpdateMotorsState();
@@ -272,8 +306,10 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimestamps)
 
   robot_driver_->UpdateMotorsState();
 
-  EXPECT_FALSE(robot_driver_->GetData(kFrontDriverName).IsMotorStatesDataTimedOut());
-  EXPECT_FALSE(robot_driver_->GetData(kRearDriverName).IsMotorStatesDataTimedOut());
+  EXPECT_FALSE(robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName)
+                 .IsMotorStatesDataTimedOut());
+  EXPECT_FALSE(
+    robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsMotorStatesDataTimedOut());
 }
 
 TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimeout)
@@ -283,10 +319,14 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimeout)
 
   panther_hardware_interfaces::MotorDriverState state = {0, 0, 0, current_time, current_time};
 
-  ON_CALL(*fl_motor_driver_, ReadMotorDriverState()).WillByDefault(::testing::Return(state));
-  ON_CALL(*fr_motor_driver_, ReadMotorDriverState()).WillByDefault(::testing::Return(state));
-  ON_CALL(*rl_motor_driver_, ReadMotorDriverState()).WillByDefault(::testing::Return(state));
-  ON_CALL(*rr_motor_driver_, ReadMotorDriverState()).WillByDefault(::testing::Return(state));
+  ON_CALL(*robot_driver_->GetMockFLMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Return(state));
+  ON_CALL(*robot_driver_->GetMockFRMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Return(state));
+  ON_CALL(*robot_driver_->GetMockRLMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Return(state));
+  ON_CALL(*robot_driver_->GetMockRRMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Return(state));
 
   // sleep for pdo_motor_states_timeout_ms + 10ms
   std::this_thread::sleep_for(
@@ -295,10 +335,12 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimeout)
 
   robot_driver_->UpdateMotorsState();
 
-  EXPECT_TRUE(robot_driver_->GetData(kFrontDriverName).IsMotorStatesDataTimedOut());
-  EXPECT_TRUE(robot_driver_->GetData(kRearDriverName).IsMotorStatesDataTimedOut());
-  EXPECT_TRUE(robot_driver_->GetData(kFrontDriverName).IsError());
-  EXPECT_TRUE(robot_driver_->GetData(kRearDriverName).IsError());
+  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName)
+                .IsMotorStatesDataTimedOut());
+  EXPECT_TRUE(
+    robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsMotorStatesDataTimedOut());
+  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName).IsError());
+  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsError());
 }
 
 TEST_F(TestPantherRobotDriver, UpdateDriverState)
@@ -325,7 +367,7 @@ TEST_F(TestPantherRobotDriver, UpdateDriverState)
   const std::uint8_t runtime_error_forward_limit_triggered = static_cast<std::uint8_t>(0b10000);
   const std::uint8_t runtime_error_reverse_limit_triggered = static_cast<std::uint8_t>(0b100000);
 
-  ON_CALL(*front_driver_mock_, ReadDriverState())
+  ON_CALL(*robot_driver_->GetMockFrontDriver(), ReadDriverState())
     .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
       {fault_flag_overheat,
        script_flag_encoder_disconnected,
@@ -338,7 +380,7 @@ TEST_F(TestPantherRobotDriver, UpdateDriverState)
        f_heatsink_temp,
        {0, 0},
        {0, 0}})));
-  ON_CALL(*rear_driver_mock_, ReadDriverState())
+  ON_CALL(*robot_driver_->GetMockRearDriver(), ReadDriverState())
     .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
       {fault_flag_overvoltage,
        script_flag_amp_limiter,
@@ -354,10 +396,12 @@ TEST_F(TestPantherRobotDriver, UpdateDriverState)
 
   robot_driver_->UpdateDriversState();
 
-  const auto & front = robot_driver_->GetData(kFrontDriverName);
-  const auto & rear = robot_driver_->GetData(kRearDriverName);
-  const auto & front_driver_state = robot_driver_->GetData(kFrontDriverName).GetDriverState();
-  const auto & rear_driver_state = robot_driver_->GetData(kRearDriverName).GetDriverState();
+  const auto & front_data = robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName);
+  const auto & rear_data = robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName);
+  const auto & front_driver_state =
+    robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName).GetDriverState();
+  const auto & rear_driver_state =
+    robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).GetDriverState();
 
   EXPECT_EQ(static_cast<std::int16_t>(front_driver_state.GetTemperature()), f_temp);
   EXPECT_EQ(
@@ -374,16 +418,18 @@ TEST_F(TestPantherRobotDriver, UpdateDriverState)
     static_cast<std::int16_t>(rear_driver_state.GetCurrent() * 10.0),
     r_battery_current_1 + r_battery_current_2);
 
-  EXPECT_TRUE(front.GetFaultFlag().GetMessage().overheat);
-  EXPECT_TRUE(front.GetScriptFlag().GetMessage().encoder_disconnected);
-  EXPECT_TRUE(front.GetRuntimeError(PantherMotorChannel::RIGHT).GetMessage().loop_error);
-  EXPECT_TRUE(front.GetRuntimeError(PantherMotorChannel::LEFT).GetMessage().safety_stop_active);
+  EXPECT_TRUE(front_data.GetFaultFlag().GetMessage().overheat);
+  EXPECT_TRUE(front_data.GetScriptFlag().GetMessage().encoder_disconnected);
+  EXPECT_TRUE(front_data.GetRuntimeError(PantherMotorChannel::RIGHT).GetMessage().loop_error);
+  EXPECT_TRUE(
+    front_data.GetRuntimeError(PantherMotorChannel::LEFT).GetMessage().safety_stop_active);
 
-  EXPECT_TRUE(rear.GetFaultFlag().GetMessage().overvoltage);
-  EXPECT_TRUE(rear.GetScriptFlag().GetMessage().amp_limiter);
+  EXPECT_TRUE(rear_data.GetFaultFlag().GetMessage().overvoltage);
+  EXPECT_TRUE(rear_data.GetScriptFlag().GetMessage().amp_limiter);
   EXPECT_TRUE(
-    rear.GetRuntimeError(PantherMotorChannel::RIGHT).GetMessage().forward_limit_triggered);
-  EXPECT_TRUE(rear.GetRuntimeError(PantherMotorChannel::LEFT).GetMessage().reverse_limit_triggered);
+    rear_data.GetRuntimeError(PantherMotorChannel::RIGHT).GetMessage().forward_limit_triggered);
+  EXPECT_TRUE(
+    rear_data.GetRuntimeError(PantherMotorChannel::LEFT).GetMessage().reverse_limit_triggered);
 }
 
 TEST_F(TestPantherRobotDriver, UpdateDriverStateTimestamps)
@@ -395,9 +441,9 @@ TEST_F(TestPantherRobotDriver, UpdateDriverStateTimestamps)
     return state;
   };
 
-  ON_CALL(*front_driver_mock_, ReadDriverState())
+  ON_CALL(*robot_driver_->GetMockFrontDriver(), ReadDriverState())
     .WillByDefault(::testing::Invoke(read_driver_state_method));
-  ON_CALL(*rear_driver_mock_, ReadDriverState())
+  ON_CALL(*robot_driver_->GetMockRearDriver(), ReadDriverState())
     .WillByDefault(::testing::Invoke(read_driver_state_method));
 
   robot_driver_->UpdateDriversState();
@@ -409,8 +455,10 @@ TEST_F(TestPantherRobotDriver, UpdateDriverStateTimestamps)
 
   robot_driver_->UpdateDriversState();
 
-  EXPECT_FALSE(robot_driver_->GetData(kFrontDriverName).IsDriverStateDataTimedOut());
-  EXPECT_FALSE(robot_driver_->GetData(kRearDriverName).IsDriverStateDataTimedOut());
+  EXPECT_FALSE(robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName)
+                 .IsDriverStateDataTimedOut());
+  EXPECT_FALSE(
+    robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsDriverStateDataTimedOut());
 }
 
 TEST_F(TestPantherRobotDriver, UpdateDriverStateTimeout)
@@ -421,8 +469,10 @@ TEST_F(TestPantherRobotDriver, UpdateDriverStateTimeout)
   panther_hardware_interfaces::DriverState state = {
     0, 0, 0, 0, 0, 0, 0, 0, 0, current_time, current_time};
 
-  ON_CALL(*front_driver_mock_, ReadDriverState()).WillByDefault(::testing::Return(state));
-  ON_CALL(*rear_driver_mock_, ReadDriverState()).WillByDefault(::testing::Return(state));
+  ON_CALL(*robot_driver_->GetMockFrontDriver(), ReadDriverState())
+    .WillByDefault(::testing::Return(state));
+  ON_CALL(*robot_driver_->GetMockRearDriver(), ReadDriverState())
+    .WillByDefault(::testing::Return(state));
 
   // sleep for pdo_driver_state_timeout_ms + 10ms
   std::this_thread::sleep_for(
@@ -431,10 +481,12 @@ TEST_F(TestPantherRobotDriver, UpdateDriverStateTimeout)
 
   robot_driver_->UpdateDriversState();
 
-  EXPECT_TRUE(robot_driver_->GetData(kFrontDriverName).IsDriverStateDataTimedOut());
-  EXPECT_TRUE(robot_driver_->GetData(kRearDriverName).IsDriverStateDataTimedOut());
-  EXPECT_TRUE(robot_driver_->GetData(kFrontDriverName).IsError());
-  EXPECT_TRUE(robot_driver_->GetData(kRearDriverName).IsError());
+  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName)
+                .IsDriverStateDataTimedOut());
+  EXPECT_TRUE(
+    robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsDriverStateDataTimedOut());
+  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName).IsError());
+  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsError());
 }
 
 TEST_F(TestPantherRobotDriver, SendSpeedCommands)
@@ -447,78 +499,78 @@ TEST_F(TestPantherRobotDriver, SendSpeedCommands)
   const float rr_v = 0.4;
 
   EXPECT_CALL(
-    *fl_motor_driver_,
+    *robot_driver_->GetMockFLMotorDriver(),
     SendCmdVel(::testing::Eq(static_cast<std::int32_t>(fl_v * kRadPerSecToRbtqCmd))))
     .Times(1);
   EXPECT_CALL(
-    *fr_motor_driver_,
+    *robot_driver_->GetMockFRMotorDriver(),
     SendCmdVel(::testing::Eq(static_cast<std::int32_t>(fr_v * kRadPerSecToRbtqCmd))))
     .Times(1);
   EXPECT_CALL(
-    *rl_motor_driver_,
+    *robot_driver_->GetMockRLMotorDriver(),
     SendCmdVel(::testing::Eq(static_cast<std::int32_t>(rl_v * kRadPerSecToRbtqCmd))))
     .Times(1);
   EXPECT_CALL(
-    *rr_motor_driver_,
+    *robot_driver_->GetMockRRMotorDriver(),
     SendCmdVel(::testing::Eq(static_cast<std::int32_t>(rr_v * kRadPerSecToRbtqCmd))))
     .Times(1);
 
-  ASSERT_NO_THROW(robot_driver_->SendSpeedCommands(fl_v, fr_v, rl_v, rr_v));
+  EXPECT_NO_THROW(robot_driver_->SendSpeedCommands(fl_v, fr_v, rl_v, rr_v));
 }
 
 TEST_F(TestPantherRobotDriver, TurnOnEStop)
 {
-  EXPECT_CALL(*front_driver_mock_, TurnOnEStop()).Times(1);
-  EXPECT_CALL(*rear_driver_mock_, TurnOnEStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOnEStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOnEStop()).Times(1);
 
-  ASSERT_NO_THROW(robot_driver_->TurnOnEStop());
+  EXPECT_NO_THROW(robot_driver_->TurnOnEStop());
 }
 
 TEST_F(TestPantherRobotDriver, TurnOffEStop)
 {
-  EXPECT_CALL(*front_driver_mock_, TurnOffEStop()).Times(1);
-  EXPECT_CALL(*rear_driver_mock_, TurnOffEStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOffEStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOffEStop()).Times(1);
 
-  ASSERT_NO_THROW(robot_driver_->TurnOffEStop());
+  EXPECT_NO_THROW(robot_driver_->TurnOffEStop());
 }
 
 TEST_F(TestPantherRobotDriver, TurnOnEStopError)
 {
-  EXPECT_CALL(*front_driver_mock_, TurnOnEStop())
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOnEStop())
     .WillOnce(::testing::Throw(std::runtime_error("")));
-  EXPECT_CALL(*rear_driver_mock_, TurnOnEStop()).Times(0);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOnEStop()).Times(0);
 
-  ASSERT_THROW(robot_driver_->TurnOnEStop(), std::runtime_error);
+  EXPECT_THROW(robot_driver_->TurnOnEStop(), std::runtime_error);
 }
 
 TEST_F(TestPantherRobotDriver, TurnOffEStopError)
 {
-  EXPECT_CALL(*front_driver_mock_, TurnOffEStop())
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOffEStop())
     .WillOnce(::testing::Throw(std::runtime_error("")));
-  EXPECT_CALL(*rear_driver_mock_, TurnOffEStop()).Times(0);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOffEStop()).Times(0);
 
-  ASSERT_THROW(robot_driver_->TurnOffEStop(), std::runtime_error);
+  EXPECT_THROW(robot_driver_->TurnOffEStop(), std::runtime_error);
 }
 
 TEST_F(TestPantherRobotDriver, SafetyStop)
 {
-  EXPECT_CALL(*fl_motor_driver_, TurnOnSafetyStop()).Times(1);
-  EXPECT_CALL(*fr_motor_driver_, TurnOnSafetyStop()).Times(1);
-  EXPECT_CALL(*rl_motor_driver_, TurnOnSafetyStop()).Times(1);
-  EXPECT_CALL(*rr_motor_driver_, TurnOnSafetyStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFLMotorDriver(), TurnOnSafetyStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFRMotorDriver(), TurnOnSafetyStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRLMotorDriver(), TurnOnSafetyStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRRMotorDriver(), TurnOnSafetyStop()).Times(1);
 
-  ASSERT_NO_THROW(robot_driver_->TurnOnSafetyStop());
+  EXPECT_NO_THROW(robot_driver_->TurnOnSafetyStop());
 }
 
 TEST_F(TestPantherRobotDriver, SafetyStopError)
 {
-  EXPECT_CALL(*fl_motor_driver_, TurnOnSafetyStop())
+  EXPECT_CALL(*robot_driver_->GetMockFLMotorDriver(), TurnOnSafetyStop())
     .WillOnce(::testing::Throw(std::runtime_error("")));
-  EXPECT_CALL(*fr_motor_driver_, TurnOnSafetyStop()).Times(0);
-  EXPECT_CALL(*rl_motor_driver_, TurnOnSafetyStop()).Times(0);
-  EXPECT_CALL(*rr_motor_driver_, TurnOnSafetyStop()).Times(0);
+  EXPECT_CALL(*robot_driver_->GetMockFRMotorDriver(), TurnOnSafetyStop()).Times(0);
+  EXPECT_CALL(*robot_driver_->GetMockRLMotorDriver(), TurnOnSafetyStop()).Times(0);
+  EXPECT_CALL(*robot_driver_->GetMockRRMotorDriver(), TurnOnSafetyStop()).Times(0);
 
-  ASSERT_THROW(robot_driver_->TurnOnSafetyStop(), std::runtime_error);
+  EXPECT_THROW(robot_driver_->TurnOnSafetyStop(), std::runtime_error);
 }
 
 int main(int argc, char ** argv)
diff --git a/panther_hardware_interfaces/test/utils/mock_driver.hpp b/panther_hardware_interfaces/test/utils/mock_driver.hpp
new file mode 100644
index 00000000..eb354a63
--- /dev/null
+++ b/panther_hardware_interfaces/test/utils/mock_driver.hpp
@@ -0,0 +1,69 @@
+// 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_HARDWARE_INTERFACES_TEST_UTILS_MOCK_DRIVER_HPP_
+#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_MOCK_DRIVER_HPP_
+
+#include <cstdint>
+#include <future>
+#include <memory>
+#include <string>
+
+#include <gmock/gmock.h>
+
+#include "panther_hardware_interfaces/panther_system/robot_driver/driver.hpp"
+
+namespace panther_hardware_interfaces_test
+{
+
+class MockDriver : public panther_hardware_interfaces::Driver
+{
+public:
+  MOCK_METHOD(std::future<void>, Boot, (), (override));
+  MOCK_METHOD(bool, IsCANError, (), (const, override));
+  MOCK_METHOD(bool, IsHeartbeatTimeout, (), (const, override));
+
+  MOCK_METHOD(panther_hardware_interfaces::DriverState, ReadDriverState, (), (override));
+  MOCK_METHOD(void, ResetScript, (), (override));
+  MOCK_METHOD(void, TurnOnEStop, (), (override));
+  MOCK_METHOD(void, TurnOffEStop, (), (override));
+
+  std::shared_ptr<panther_hardware_interfaces::MotorDriver> GetMotorDriver(
+    const std::string & name) override
+  {
+    return motor_drivers_.at(name);
+  }
+
+  void AddMotorDriver(
+    const std::string name,
+    std::shared_ptr<panther_hardware_interfaces::MotorDriver> motor_driver) override
+  {
+    motor_drivers_.emplace(name, motor_driver);
+  }
+
+private:
+  std::map<std::string, std::shared_ptr<panther_hardware_interfaces::MotorDriver>> motor_drivers_;
+};
+
+class MockMotorDriver : public panther_hardware_interfaces::MotorDriver
+{
+public:
+  MOCK_METHOD(panther_hardware_interfaces::MotorDriverState, ReadMotorDriverState, (), (override));
+  MOCK_METHOD(void, SendCmdVel, (const std::int32_t cmd), (override));
+  MOCK_METHOD(void, TurnOnSafetyStop, (), (override));
+};
+
+}  // namespace panther_hardware_interfaces_test
+
+#endif  // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_MOCK_DRIVER_HPP_

From bc80dae4bb7238f86f469f2c15ddc804a5205e25 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Wed, 4 Sep 2024 07:22:21 +0000
Subject: [PATCH 035/100] CANopenSettings update

---
 .../panther_system/robot_driver/canopen_manager.hpp           | 3 +--
 .../src/panther_system/robot_driver/lynx_robot_driver.cpp     | 2 +-
 .../src/panther_system/robot_driver/panther_robot_driver.cpp  | 4 ++--
 panther_hardware_interfaces/test/utils/test_constants.hpp     | 3 +--
 4 files changed, 5 insertions(+), 7 deletions(-)

diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp
index 33dcf9c7..2c00eb69 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp
@@ -47,8 +47,7 @@ struct CANopenSettings
   std::string can_interface_name;
 
   std::uint8_t master_can_id;
-  std::uint8_t front_driver_can_id;
-  std::uint8_t rear_driver_can_id;
+  std::map<std::string, std::uint8_t> driver_can_ids;
 
   std::chrono::milliseconds pdo_motor_states_timeout_ms;
   std::chrono::milliseconds pdo_driver_state_timeout_ms;
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
index 786d0156..32010c69 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
@@ -199,7 +199,7 @@ void LynxRobotDriver::TurnOnSafetyStop()
 void LynxRobotDriver::DefineDriver()
 {
   driver_ = std::make_shared<RoboteqDriver>(
-    canopen_manager_.GetMaster(), canopen_settings_.front_driver_can_id,
+    canopen_manager_.GetMaster(), canopen_settings_.driver_can_ids.at(LynxDriverNames::DEFAULT),
     canopen_settings_.sdo_operation_timeout_ms);
 
   auto left_motor_driver = std::make_shared<RoboteqMotorDriver>(
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp
index 11a99135..c9e46a57 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp
@@ -259,10 +259,10 @@ void PantherRobotDriver::TurnOnSafetyStop()
 void PantherRobotDriver::DefineDrivers()
 {
   front_driver_ = std::make_shared<RoboteqDriver>(
-    canopen_manager_.GetMaster(), canopen_settings_.front_driver_can_id,
+    canopen_manager_.GetMaster(), canopen_settings_.driver_can_ids.at(PantherDriverNames::FRONT),
     canopen_settings_.sdo_operation_timeout_ms);
   rear_driver_ = std::make_shared<RoboteqDriver>(
-    canopen_manager_.GetMaster(), canopen_settings_.rear_driver_can_id,
+    canopen_manager_.GetMaster(), canopen_settings_.driver_can_ids.at(PantherDriverNames::REAR),
     canopen_settings_.sdo_operation_timeout_ms);
 
   auto fl_motor_driver = std::make_shared<RoboteqMotorDriver>(
diff --git a/panther_hardware_interfaces/test/utils/test_constants.hpp b/panther_hardware_interfaces/test/utils/test_constants.hpp
index ffb00322..2c34ce42 100644
--- a/panther_hardware_interfaces/test/utils/test_constants.hpp
+++ b/panther_hardware_interfaces/test/utils/test_constants.hpp
@@ -30,8 +30,7 @@ namespace panther_hardware_interfaces_test
 const panther_hardware_interfaces::CANopenSettings kCANopenSettings{
   "panther_can",
   3,
-  1,
-  2,
+  {{"default", 1}, {"front", 1}, {"rear", 2}},
   std::chrono::milliseconds(15),
   std::chrono::milliseconds(75),
   std::chrono::milliseconds(100),

From 1f6cce9fb3e64dc32e55a3501da9039dc78b7b20 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Wed, 4 Sep 2024 07:55:46 +0000
Subject: [PATCH 036/100] uncomment cmakelists

---
 panther_hardware_interfaces/CMakeLists.txt | 129 ++++++++++++---------
 1 file changed, 71 insertions(+), 58 deletions(-)

diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt
index 681f4287..4ecfa603 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/panther_hardware_interfaces/CMakeLists.txt
@@ -51,19 +51,18 @@ pkg_check_modules(LIBLELY_COAPP REQUIRED IMPORTED_TARGET liblely-coapp)
 pkg_check_modules(LIBGPIOD REQUIRED IMPORTED_TARGET libgpiodcxx)
 
 add_library(
-  ${PROJECT_NAME}
-  SHARED
-  # src/panther_imu_sensor/panther_imu_sensor.cpp
-  # src/panther_system/gpio/gpio_controller.cpp
-  # src/panther_system/gpio/gpio_driver.cpp
-  # src/panther_system/robot_driver/canopen_manager.cpp
-  # src/panther_system/robot_driver/robot_driver.cpp
-  # src/panther_system/robot_driver/roboteq_data_converters.cpp
-  # src/panther_system/robot_driver/roboteq_driver.cpp
-  # src/panther_system/robot_driver/roboteq_error_filter.cpp
-  # src/panther_system/panther_system_e_stop.cpp
-  # src/panther_system/panther_system_ros_interface.cpp
-  # src/panther_system/panther_system.cpp
+  ${PROJECT_NAME} SHARED
+  src/panther_imu_sensor/panther_imu_sensor.cpp
+  src/panther_system/gpio/gpio_controller.cpp
+  src/panther_system/gpio/gpio_driver.cpp
+  src/panther_system/robot_driver/canopen_manager.cpp
+  src/panther_system/robot_driver/robot_driver.cpp
+  src/panther_system/robot_driver/roboteq_data_converters.cpp
+  src/panther_system/robot_driver/roboteq_driver.cpp
+  src/panther_system/robot_driver/roboteq_error_filter.cpp
+  src/panther_system/panther_system_e_stop.cpp
+  src/panther_system/panther_system_ros_interface.cpp
+  src/panther_system/panther_system.cpp
   src/utils.cpp)
 ament_target_dependencies(${PROJECT_NAME} ${PACKAGE_DEPENDENCIES})
 target_link_libraries(${PROJECT_NAME} PkgConfig::LIBLELY_COAPP
@@ -90,15 +89,15 @@ if(BUILD_TESTING)
 
   install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)
 
-  # ament_add_gtest(${PROJECT_NAME}_test_utils test/test_utils.cpp
-  # src/utils.cpp)
+  ament_add_gtest(${PROJECT_NAME}_test_utils test/test_utils.cpp src/utils.cpp)
 
-  # ament_add_gtest(${PROJECT_NAME}_test_panther_imu_sensor
-  # test/panther_imu_sensor/test_panther_imu_sensor.cpp)
-  # ament_target_dependencies( ${PROJECT_NAME}_test_panther_imu_sensor
-  # hardware_interface rclcpp panther_utils panther_msgs phidgets_api)
-  # target_link_libraries(${PROJECT_NAME}_test_panther_imu_sensor
-  # ${PROJECT_NAME} phidgets_spatial_parameters)
+  ament_add_gtest(${PROJECT_NAME}_test_panther_imu_sensor
+                  test/panther_imu_sensor/test_panther_imu_sensor.cpp)
+  ament_target_dependencies(
+    ${PROJECT_NAME}_test_panther_imu_sensor hardware_interface rclcpp
+    panther_utils panther_msgs phidgets_api)
+  target_link_libraries(${PROJECT_NAME}_test_panther_imu_sensor ${PROJECT_NAME}
+                        phidgets_spatial_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_error_filter
@@ -180,43 +179,57 @@ if(BUILD_TESTING)
   target_link_libraries(${PROJECT_NAME}_test_lynx_robot_driver
                         PkgConfig::LIBLELY_COAPP)
 
-  # ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver
-  # test/panther_system/gpio/test_gpio_driver.cpp)
-  # ament_target_dependencies(${PROJECT_NAME}_test_gpiod_driver panther_utils)
-  # target_link_libraries(${PROJECT_NAME}_test_gpiod_driver ${PROJECT_NAME}
-  # PkgConfig::LIBGPIOD)
-
-  # ament_add_gtest( ${PROJECT_NAME}_test_gpiod_controller
-  # test/panther_system/gpio/test_gpio_controller.cpp
-  # src/panther_system/gpio/gpio_controller.cpp
-  # src/panther_system/gpio/gpio_driver.cpp)
-  # ament_target_dependencies(${PROJECT_NAME}_test_gpiod_controller
-  # panther_utils) target_link_libraries(${PROJECT_NAME}_test_gpiod_controller
-  # PkgConfig::LIBGPIOD)
-
-  # ament_add_gtest( ${PROJECT_NAME}_test_panther_system_ros_interface
-  # test/panther_system/test_panther_system_ros_interface.cpp
-  # src/panther_system/panther_system_ros_interface.cpp
-  # src/panther_system/robot_driver/roboteq_data_converters.cpp src/utils.cpp
-  # src/panther_system/gpio/gpio_controller.cpp
-  # src/panther_system/gpio/gpio_driver.cpp) target_include_directories(
-  # ${PROJECT_NAME}_test_panther_system_ros_interface PUBLIC
-  # $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
-  # $<INSTALL_INTERFACE:include>) ament_target_dependencies(
-  # ${PROJECT_NAME}_test_panther_system_ros_interface diagnostic_updater rclcpp
-  # panther_msgs panther_utils realtime_tools std_srvs)
-  # target_link_libraries(${PROJECT_NAME}_test_panther_system_ros_interface
-  # PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD)
-
-  # ament_add_gtest(${PROJECT_NAME}_test_panther_system
-  # test/panther_system/test_panther_system.cpp)
-  # set_tests_properties(${PROJECT_NAME}_test_panther_system PROPERTIES TIMEOUT
-  # 120) target_include_directories( ${PROJECT_NAME}_test_panther_system PUBLIC
-  # $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
-  # $<INSTALL_INTERFACE:include>) ament_target_dependencies(
-  # ${PROJECT_NAME}_test_panther_system hardware_interface rclcpp panther_msgs
-  # panther_utils) target_link_libraries(${PROJECT_NAME}_test_panther_system
-  # PkgConfig::LIBLELY_COAPP)
+  ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver
+                  test/panther_system/gpio/test_gpio_driver.cpp)
+  ament_target_dependencies(${PROJECT_NAME}_test_gpiod_driver panther_utils)
+  target_link_libraries(${PROJECT_NAME}_test_gpiod_driver ${PROJECT_NAME}
+                        PkgConfig::LIBGPIOD)
+
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_gpiod_controller
+    test/panther_system/gpio/test_gpio_controller.cpp
+    src/panther_system/gpio/gpio_controller.cpp
+    src/panther_system/gpio/gpio_driver.cpp)
+  ament_target_dependencies(${PROJECT_NAME}_test_gpiod_controller panther_utils)
+  target_link_libraries(${PROJECT_NAME}_test_gpiod_controller
+                        PkgConfig::LIBGPIOD)
+
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_panther_system_ros_interface
+    test/panther_system/test_panther_system_ros_interface.cpp
+    src/panther_system/panther_system_ros_interface.cpp
+    src/panther_system/robot_driver/roboteq_data_converters.cpp
+    src/utils.cpp
+    src/panther_system/gpio/gpio_controller.cpp
+    src/panther_system/gpio/gpio_driver.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_panther_system_ros_interface
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(
+    ${PROJECT_NAME}_test_panther_system_ros_interface
+    diagnostic_updater
+    rclcpp
+    panther_msgs
+    panther_utils
+    realtime_tools
+    std_srvs)
+  target_link_libraries(${PROJECT_NAME}_test_panther_system_ros_interface
+                        PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD)
+
+  ament_add_gtest(${PROJECT_NAME}_test_panther_system
+                  test/panther_system/test_panther_system.cpp)
+  set_tests_properties(${PROJECT_NAME}_test_panther_system PROPERTIES TIMEOUT
+                                                                      120)
+  target_include_directories(
+    ${PROJECT_NAME}_test_panther_system
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(
+    ${PROJECT_NAME}_test_panther_system hardware_interface rclcpp panther_msgs
+    panther_utils)
+  target_link_libraries(${PROJECT_NAME}_test_panther_system
+                        PkgConfig::LIBLELY_COAPP)
 
 endif()
 

From 5a20fe0d7aafce255e74d3a4b9ed716abff9137e Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Wed, 4 Sep 2024 11:04:34 +0000
Subject: [PATCH 037/100] add roboteq robot driver abstraction

---
 panther_hardware_interfaces/CMakeLists.txt    |  19 +
 .../robot_driver/lynx_robot_driver.hpp        | 148 +----
 .../robot_driver/panther_robot_driver.hpp     | 149 +----
 .../robot_driver/robot_driver.hpp             |  13 +-
 .../robot_driver/roboteq_robot_driver.hpp     | 176 ++++++
 .../robot_driver/lynx_robot_driver.cpp        | 215 +------
 .../robot_driver/panther_robot_driver.cpp     | 282 +--------
 .../robot_driver/roboteq_robot_driver.cpp     | 235 ++++++++
 .../robot_driver/test_lynx_robot_driver.cpp   | 331 ++--------
 .../test_panther_robot_driver.cpp             | 442 ++------------
 .../test_roboteq_robot_driver.cpp             | 569 ++++++++++++++++++
 11 files changed, 1166 insertions(+), 1413 deletions(-)
 create mode 100644 panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
 create mode 100644 panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
 create mode 100644 panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp

diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt
index 74430047..43a333eb 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/panther_hardware_interfaces/CMakeLists.txt
@@ -146,12 +146,30 @@ if(BUILD_TESTING)
   target_link_libraries(${PROJECT_NAME}_test_roboteq_driver
                         PkgConfig::LIBLELY_COAPP)
 
+  ament_add_gmock(
+    ${PROJECT_NAME}_test_roboteq_robot_driver
+    test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
+    src/panther_system/robot_driver/canopen_manager.cpp
+    src/panther_system/robot_driver/roboteq_driver.cpp
+    src/panther_system/robot_driver/roboteq_data_converters.cpp
+    src/panther_system/robot_driver/roboteq_robot_driver.cpp
+    src/utils.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_roboteq_robot_driver
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(${PROJECT_NAME}_test_roboteq_robot_driver rclcpp
+                            panther_msgs panther_utils)
+  target_link_libraries(${PROJECT_NAME}_test_roboteq_robot_driver
+                        PkgConfig::LIBLELY_COAPP)
+
   ament_add_gmock(
     ${PROJECT_NAME}_test_panther_robot_driver
     test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
     src/panther_system/robot_driver/canopen_manager.cpp
     src/panther_system/robot_driver/roboteq_driver.cpp
     src/panther_system/robot_driver/roboteq_data_converters.cpp
+    src/panther_system/robot_driver/roboteq_robot_driver.cpp
     src/panther_system/robot_driver/panther_robot_driver.cpp
     src/utils.cpp)
   target_include_directories(
@@ -169,6 +187,7 @@ if(BUILD_TESTING)
     src/panther_system/robot_driver/canopen_manager.cpp
     src/panther_system/robot_driver/roboteq_driver.cpp
     src/panther_system/robot_driver/roboteq_data_converters.cpp
+    src/panther_system/robot_driver/roboteq_robot_driver.cpp
     src/panther_system/robot_driver/lynx_robot_driver.cpp
     src/utils.cpp)
   target_include_directories(
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
index 3bc0b3df..ce6547fd 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
@@ -16,33 +16,16 @@
 #define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_LYNX_ROBOT_DRIVER_HPP_
 
 #include <chrono>
-#include <string>
+#include <memory>
+#include <vector>
 
-#include <panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp>
 #include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
 #include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
 
 namespace panther_hardware_interfaces
 {
 
-struct LynxMotorNames
-{
-  static constexpr char LEFT[] = "left";
-  static constexpr char RIGHT[] = "right";
-};
-
-struct LynxDriverNames
-{
-  static constexpr char DEFAULT[] = "default";
-};
-
-struct LynxMotorChannel
-{
-  static constexpr std::uint8_t LEFT = RoboteqDriver::kChannel2;
-  static constexpr std::uint8_t RIGHT = RoboteqDriver::kChannel1;
-};
-
 /**
  * @brief This class abstracts the usage of one Roboteq controller.
  * It uses canopen_manager for communication with Roboteq controller,
@@ -50,139 +33,40 @@ struct LynxMotorChannel
  * and provides methods to get data feedback and send commands.
  * Data is converted between raw Roboteq formats and SI units using roboteq_data_converters.
  */
-class LynxRobotDriver : public RobotDriver
+class LynxRobotDriver : public RoboteqRobotDriver
 {
 public:
   LynxRobotDriver(
     const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings,
-    const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000));
-
-  ~LynxRobotDriver()
+    const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000))
+  : RoboteqRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time)
   {
-    driver_.reset();
-    Deinitialize();
-  };
-
-  /**
-   * @brief Starts CAN communication and waits for boot to finish
-   *
-   * @exception std::runtime_error if boot fails
-   */
-  void Initialize() override;
-
-  /**
-   * @brief Deinitialize can communication
-   */
-  void Deinitialize() override;
-
-  /**
-   * @brief Activate procedure for Roboteq drivers - reset scripts and send 0 commands on both
-   * channels. Blocking function, takes around 2 seconds to finish
-   *
-   * @exception std::runtime_error if any procedure step fails
-   */
-  void Activate() override;
-
-  /**
-   * @brief Updates current communication state with Roboteq drivers
-   *
-   * @exception std::runtime_error if CAN error was detected
-   */
-  void UpdateCommunicationState() override;
-
-  /**
-   * @brief Updates current motors' state (position, velocity, current).
-   *
-   * @exception std::runtime_error if CAN error was detected
-   */
-  void UpdateMotorsState() override;
+  }
 
-  /**
-   * @brief Updates current Roboteq driver state (flags, temperatures, voltage, battery current)
-   *
-   * @exception std::runtime_error if CAN error was detected
-   */
-  void UpdateDriversState() override;
-
-  /**
-   * @brief Get data feedback from the driver
-   *
-   * @param name name of the data to get
-   *
-   * @return data feedback
-   * @exception std::runtime_error if data with the given name does not exist
-   */
-  const RoboteqData & GetData(const std::string & name) override;
+  ~LynxRobotDriver() = default;
 
   /**
    * @brief Write speed commands to motors
    *
-   * @param speed_fl front left motor speed in rad/s
-   * @param speed_fr front right motor speed in rad/s
-   * @param speed_rl rear left motor speed in rad/s
-   * @param speed_rr rear right motor speed in rad/s
-   *
-   * @exception std::runtime_error if send command fails or CAN error was detected
-   */
-  void SendSpeedCommands(
-    const float speed_left, const float speed_right, const float /*speed_rl*/,
-    const float /*speed_rr*/) override;
-
-  /**
-   * @brief Turns on Roboteq E-stop
-   *
-   * @exception std::runtime_error if any operation returns error
-   */
-  void TurnOnEStop() override;
-
-  /**
-   * @brief Turns off Roboteq E-stop
-   *
-   * @exception std::runtime_error if any operation returns error
-   */
-  void TurnOffEStop() override;
-
-  /**
-   * @brief Turns on Roboteq safety stop. To turn it off, it is necessary to send
-   * 0 commands to motors.
+   * @param speeds vector of motor speeds in rad/s. The order is: left, right
    *
-   * @exception std::runtime_error if any operation returns error
+   * @exception std::runtime_error if invalid vector size, send command fails or CAN error is
+   * detected
    */
-  void TurnOnSafetyStop() override;
+  void SendSpeedCommands(const std::vector<float> & speeds) override;
 
   /**
    * @brief Attempt to clear driver error flags by sending 0 velocity commands to motors. If Roboteq
    * driver faults still exist, the error flag will remain active.
    */
-  inline void AttemptErrorFlagResetWithZeroSpeed() { SendSpeedCommands(0.0, 0.0, 0.0, 0.0); };
+  inline void AttemptErrorFlagResetWithZeroSpeed() override { SendSpeedCommands({0.0, 0.0}); };
 
 protected:
   /**
-   * @brief This method defines driver object and adds motor drivers to it. It is virtual to allow
-   * mocking driver in tests.
+   * @brief This method defines driver objects and adds motor drivers for them. It is virtual to
+   * allow mocking drivers in tests.
    */
-  virtual void DefineDriver();
-
-  std::shared_ptr<Driver> driver_;
-
-private:
-  void SetMotorsStates(
-    RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
-    const timespec & current_time);
-  void SetDriverState(RoboteqData & data, const DriverState & state, const timespec & current_time);
-
-  bool initialized_ = false;
-
-  CANopenSettings canopen_settings_;
-  CANopenManager canopen_manager_;
-
-  RoboteqData data_;
-
-  RoboteqVelocityCommandConverter roboteq_vel_cmd_converter_;
-
-  const std::chrono::milliseconds pdo_motor_states_timeout_ms_;
-  const std::chrono::milliseconds pdo_driver_state_timeout_ms_;
-  const std::chrono::milliseconds activate_wait_time_;
+  virtual void DefineDrivers();
 };
 
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
index 2dc16981..ba401eae 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
@@ -16,34 +16,16 @@
 #define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_PANTHER_ROBOT_DRIVER_HPP_
 
 #include <chrono>
-#include <string>
+#include <memory>
+#include <vector>
 
 #include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
 #include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
 
 namespace panther_hardware_interfaces
 {
 
-struct PantherMotorNames
-{
-  static constexpr char LEFT[] = "left";
-  static constexpr char RIGHT[] = "right";
-};
-
-struct PantherDriverNames
-{
-  static constexpr char FRONT[] = "front";
-  static constexpr char REAR[] = "rear";
-};
-
-struct PantherMotorChannel
-{
-  static constexpr std::uint8_t LEFT = RoboteqDriver::kChannel2;
-  static constexpr std::uint8_t RIGHT = RoboteqDriver::kChannel1;
-};
-
 /**
  * @brief This class abstracts the usage of two Roboteq controllers.
  * It uses canopen_manager for communication with Roboteq controllers,
@@ -51,111 +33,37 @@ struct PantherMotorChannel
  * and provides methods to get data feedback and send commands.
  * Data is converted between raw Roboteq formats and SI units using roboteq_data_converters.
  */
-class PantherRobotDriver : public RobotDriver
+class PantherRobotDriver : public RoboteqRobotDriver
 {
 public:
   PantherRobotDriver(
     const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings,
-    const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000));
-
-  ~PantherRobotDriver()
+    const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000))
+  : RoboteqRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time)
   {
-    front_driver_.reset();
-    rear_driver_.reset();
-    Deinitialize();
-  };
-
-  /**
-   * @brief Starts CAN communication and waits for boot to finish
-   *
-   * @exception std::runtime_error if boot fails
-   */
-  void Initialize() override;
+  }
 
-  /**
-   * @brief Deinitialize can communication
-   */
-  void Deinitialize() override;
-
-  /**
-   * @brief Activate procedure for Roboteq drivers - reset scripts and send 0 commands on both
-   * channels. Blocking function, takes around 2 seconds to finish
-   *
-   * @exception std::runtime_error if any procedure step fails
-   */
-  void Activate() override;
-
-  /**
-   * @brief Updates current communication state with Roboteq drivers
-   *
-   * @exception std::runtime_error if CAN error was detected
-   */
-  void UpdateCommunicationState() override;
-
-  /**
-   * @brief Updates current motors' state (position, velocity, current).
-   *
-   * @exception std::runtime_error if CAN error was detected
-   */
-  void UpdateMotorsState() override;
-
-  /**
-   * @brief Updates current Roboteq driver state (flags, temperatures, voltage, battery current)
-   *
-   * @exception std::runtime_error if CAN error was detected
-   */
-  void UpdateDriversState() override;
-
-  /**
-   * @brief Get data feedback from the driver
-   *
-   * @param name name of the data to get
-   *
-   * @return data feedback
-   * @exception std::runtime_error if data with the given name does not exist
-   */
-  const RoboteqData & GetData(const std::string & name) override;
+  ~PantherRobotDriver() = default;
 
   /**
    * @brief Write speed commands to motors
    *
-   * @param speed_fl front left motor speed in rad/s
-   * @param speed_fr front right motor speed in rad/s
-   * @param speed_rl rear left motor speed in rad/s
-   * @param speed_rr rear right motor speed in rad/s
+   * @param speeds vector of motor speeds in rad/s.
+   * The order is: front left, front right, rear left, rear right
    *
-   * @exception std::runtime_error if send command fails or CAN error was detected
+   * @exception std::runtime_error if invalid vector size, send command fails or CAN error is
+   * detected
    */
-  void SendSpeedCommands(
-    const float speed_fl, const float speed_fr, const float speed_rl, const float speed_rr);
-
-  /**
-   * @brief Turns on Roboteq E-stop
-   *
-   * @exception std::runtime_error if any operation returns error
-   */
-  void TurnOnEStop();
-
-  /**
-   * @brief Turns off Roboteq E-stop
-   *
-   * @exception std::runtime_error if any operation returns error
-   */
-  void TurnOffEStop();
-
-  /**
-   * @brief Turns on Roboteq safety stop. To turn it off, it is necessary to send
-   * 0 commands to motors.
-   *
-   * @exception std::runtime_error if any operation returns error
-   */
-  void TurnOnSafetyStop();
+  void SendSpeedCommands(const std::vector<float> & speeds) override;
 
   /**
    * @brief Attempt to clear driver error flags by sending 0 velocity commands to motors. If Roboteq
    * driver faults still exist, the error flag will remain active.
    */
-  inline void AttemptErrorFlagResetWithZeroSpeed() { SendSpeedCommands(0.0, 0.0, 0.0, 0.0); };
+  inline void AttemptErrorFlagResetWithZeroSpeed() override
+  {
+    SendSpeedCommands({0.0, 0.0, 0.0, 0.0});
+  };
 
 protected:
   /**
@@ -163,29 +71,6 @@ class PantherRobotDriver : public RobotDriver
    * allow mocking drivers in tests.
    */
   virtual void DefineDrivers();
-
-  std::shared_ptr<Driver> front_driver_;
-  std::shared_ptr<Driver> rear_driver_;
-
-private:
-  void SetMotorsStates(
-    RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
-    const timespec & current_time);
-  void SetDriverState(RoboteqData & data, const DriverState & state, const timespec & current_time);
-
-  bool initialized_ = false;
-
-  CANopenSettings canopen_settings_;
-  CANopenManager canopen_manager_;
-
-  RoboteqData front_data_;
-  RoboteqData rear_data_;
-
-  RoboteqVelocityCommandConverter roboteq_vel_cmd_converter_;
-
-  const std::chrono::milliseconds pdo_motor_states_timeout_ms_;
-  const std::chrono::milliseconds pdo_driver_state_timeout_ms_;
-  const std::chrono::milliseconds activate_wait_time_;
 };
 
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
index b972f33b..02a2cead 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
@@ -16,6 +16,7 @@
 #define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_
 
 #include <string>
+#include <vector>
 
 #include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
 
@@ -81,15 +82,11 @@ class RobotDriver
   /**
    * @brief Write speed commands to motors
    *
-   * @param speed_fl front left motor speed in rad/s
-   * @param speed_fr front right motor speed in rad/s
-   * @param speed_rl rear left motor speed in rad/s
-   * @param speed_rr rear right motor speed in rad/s
+   * @param speeds vector of motor speeds in rad/s
    *
-   * @exception std::runtime_error if send command fails
+   * @exception std::runtime_error if vector has invalid size or send command fails
    */
-  virtual void SendSpeedCommands(
-    const float speed_fl, const float speed_fr, const float speed_rl, const float speed_rr) = 0;
+  virtual void SendSpeedCommands(const std::vector<float> & speeds) = 0;
 
   /**
    * @brief Turns on E-stop
@@ -117,7 +114,7 @@ class RobotDriver
    * @brief Attempt to clear driver error flags by sending 0 velocity commands to motors. If driver
    * faults still exist, the error flag will remain active.
    */
-  inline void AttemptErrorFlagResetWithZeroSpeed() { SendSpeedCommands(0.0, 0.0, 0.0, 0.0); };
+  virtual inline void AttemptErrorFlagResetWithZeroSpeed() = 0;
 };
 
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
new file mode 100644
index 00000000..1f1cd062
--- /dev/null
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
@@ -0,0 +1,176 @@
+// 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_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ROBOT_DRIVER_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ROBOT_DRIVER_HPP_
+
+#include <chrono>
+#include <string>
+#include <vector>
+
+#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
+
+namespace panther_hardware_interfaces
+{
+
+struct MotorNames
+{
+  static constexpr char LEFT[] = "left";
+  static constexpr char RIGHT[] = "right";
+};
+
+struct DriverNames
+{
+  static constexpr char DEFAULT[] = "default";
+  static constexpr char FRONT[] = "front";
+  static constexpr char REAR[] = "rear";
+};
+
+struct MotorChannels
+{
+  static constexpr std::uint8_t LEFT = RoboteqDriver::kChannel2;
+  static constexpr std::uint8_t RIGHT = RoboteqDriver::kChannel1;
+};
+
+/**
+ * @brief Abstract class for use with Roboteq controllers.
+ * It uses canopen_manager for communication with Roboteq controllers,
+ * implements the activation procedure for controllers (resets script and sends initial 0 command),
+ * and provides methods to get data feedback and send commands.
+ * Data is converted between raw Roboteq formats and SI units using roboteq_data_converters.
+ */
+class RoboteqRobotDriver : public RobotDriver
+{
+public:
+  RoboteqRobotDriver(
+    const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings,
+    const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000));
+
+  ~RoboteqRobotDriver()
+  {
+    drivers_.clear();
+    Deinitialize();
+  };
+
+  /**
+   * @brief Starts CAN communication and waits for boot to finish
+   *
+   * @exception std::runtime_error if boot fails
+   */
+  void Initialize() override;
+
+  /**
+   * @brief Deinitialize can communication
+   */
+  void Deinitialize() override;
+
+  /**
+   * @brief Activate procedure for Roboteq drivers - reset scripts and send 0 commands on both
+   * channels. Blocking function, takes around 2 seconds to finish
+   *
+   * @exception std::runtime_error if any procedure step fails
+   */
+  void Activate() override;
+
+  /**
+   * @brief Updates current communication state with Roboteq drivers
+   *
+   * @exception std::runtime_error if CAN error was detected
+   */
+  void UpdateCommunicationState() override;
+
+  /**
+   * @brief Updates current motors' state (position, velocity, current).
+   *
+   * @exception std::runtime_error if CAN error was detected
+   */
+  void UpdateMotorsState() override;
+
+  /**
+   * @brief Updates current Roboteq driver state (flags, temperatures, voltage, battery current)
+   *
+   * @exception std::runtime_error if CAN error was detected
+   */
+  void UpdateDriversState() override;
+
+  /**
+   * @brief Turns on Roboteq E-stop
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  void TurnOnEStop() override;
+
+  /**
+   * @brief Turns off Roboteq E-stop
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  void TurnOffEStop() override;
+
+  /**
+   * @brief Turns on Roboteq safety stop. To turn it off, it is necessary to send
+   * 0 commands to motors.
+   *
+   * @exception std::runtime_error if any operation returns error
+   */
+  void TurnOnSafetyStop() override;
+
+  /**
+   * @brief Get data feedback from the driver
+   *
+   * @param name name of the data to get
+   *
+   * @return data feedback
+   * @exception std::runtime_error if data with the given name does not exist
+   */
+  const RoboteqData & GetData(const std::string & name) override;
+
+protected:
+  /**
+   * @brief This method defines driver objects and adds motor drivers for them.
+   */
+  virtual void DefineDrivers() = 0;
+
+  RoboteqVelocityCommandConverter & GetCmdVelConverter() { return roboteq_vel_cmd_converter_; }
+  CANopenSettings & GetCANopenSettings() { return canopen_settings_; }
+  CANopenManager & GetCANopenManager() { return canopen_manager_; }
+
+  CANopenSettings canopen_settings_;
+  DrivetrainSettings drivetrain_settings_;
+
+  CANopenManager canopen_manager_;
+  std::map<std::string, std::shared_ptr<Driver>> drivers_;
+
+private:
+  void SetMotorsStates(
+    RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
+    const timespec & current_time);
+  void SetDriverState(RoboteqData & data, const DriverState & state, const timespec & current_time);
+
+  bool initialized_ = false;
+
+  std::map<std::string, RoboteqData> data_;
+  RoboteqVelocityCommandConverter roboteq_vel_cmd_converter_;
+
+  const std::chrono::milliseconds pdo_motor_states_timeout_ms_;
+  const std::chrono::milliseconds pdo_driver_state_timeout_ms_;
+  const std::chrono::milliseconds activate_wait_time_;
+};
+
+}  // namespace panther_hardware_interfaces
+
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ROBOT_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
index 32010c69..a86caa5e 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
@@ -14,230 +14,55 @@
 
 #include "panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp"
 
-#include <chrono>
-#include <ctime>
 #include <memory>
 #include <stdexcept>
 #include <string>
-#include <thread>
-#include <utility>
+#include <vector>
 
-#include "lely/util/chrono.hpp"
-
-#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
 #include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
 
 namespace panther_hardware_interfaces
 {
 
-LynxRobotDriver::LynxRobotDriver(
-  const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings,
-  const std::chrono::milliseconds activate_wait_time)
-: canopen_settings_(canopen_settings),
-  canopen_manager_(canopen_settings),
-  data_(drivetrain_settings),
-  roboteq_vel_cmd_converter_(drivetrain_settings),
-  pdo_motor_states_timeout_ms_(canopen_settings.pdo_motor_states_timeout_ms),
-  pdo_driver_state_timeout_ms_(canopen_settings.pdo_driver_state_timeout_ms),
-  activate_wait_time_(activate_wait_time)
-{
-}
-
-void LynxRobotDriver::Initialize()
-{
-  if (initialized_) {
-    return;
-  }
-
-  try {
-    canopen_manager_.Initialize();
-    DefineDriver();
-    driver_->Boot();
-  } catch (const std::runtime_error & e) {
-    throw e;
-  }
-
-  initialized_ = true;
-}
-
-void LynxRobotDriver::Deinitialize()
-{
-  canopen_manager_.Deinitialize();
-  initialized_ = false;
-}
-
-void LynxRobotDriver::Activate()
-{
-  // Activation procedure - it is necessary to first reset scripts, wait for a bit (1 second)
-  // and then send 0 commands for some time (also 1 second)
-
-  try {
-    driver_->ResetScript();
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Front driver reset Roboteq script exception: " + std::string(e.what()));
-  }
-
-  std::this_thread::sleep_for(activate_wait_time_);
-
-  try {
-    driver_->GetMotorDriver(LynxMotorNames::LEFT)->SendCmdVel(0);
-    driver_->GetMotorDriver(LynxMotorNames::RIGHT)->SendCmdVel(0);
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error("Front driver send 0 command exception: " + std::string(e.what()));
-  }
-
-  std::this_thread::sleep_for(activate_wait_time_);
-}
-
-void LynxRobotDriver::UpdateCommunicationState()
-{
-  data_.SetCANError(driver_->IsCANError());
-  data_.SetHeartbeatTimeout(driver_->IsHeartbeatTimeout());
-}
-
-void LynxRobotDriver::UpdateMotorsState()
-{
-  timespec current_time;
-  clock_gettime(CLOCK_MONOTONIC, &current_time);
-
-  const auto fl_state = driver_->GetMotorDriver(LynxMotorNames::LEFT)->ReadMotorDriverState();
-  const auto fr_state = driver_->GetMotorDriver(LynxMotorNames::RIGHT)->ReadMotorDriverState();
-
-  SetMotorsStates(data_, fl_state, fr_state, current_time);
-
-  UpdateCommunicationState();
-
-  if (data_.IsCANError()) {
-    throw std::runtime_error("CAN error.");
-  }
-
-  if (data_.IsHeartbeatTimeout()) {
-    throw std::runtime_error("Motor controller heartbeat timeout.");
-  }
-}
-
-void LynxRobotDriver::UpdateDriversState()
-{
-  timespec current_time;
-  clock_gettime(CLOCK_MONOTONIC, &current_time);
-
-  SetDriverState(data_, driver_->ReadDriverState(), current_time);
-
-  UpdateCommunicationState();
-
-  if (data_.IsCANError()) {
-    throw std::runtime_error("CAN error.");
-  }
-
-  if (data_.IsHeartbeatTimeout()) {
-    throw std::runtime_error("Motor controller heartbeat timeout.");
-  }
-}
-
-const RoboteqData & LynxRobotDriver::GetData(const std::string & name)
-{
-  if (name == LynxDriverNames::DEFAULT) {
-    return data_;
-  } else {
-    throw std::runtime_error("Data with name '" + name + "' does not exist.");
-  }
-}
-
-void LynxRobotDriver::SendSpeedCommands(
-  const float speed_left, const float speed_right, const float /*speed_rl*/,
-  const float /*speed_rr*/)
+void LynxRobotDriver::SendSpeedCommands(const std::vector<float> & speeds)
 {
-  // Channel 1 - right motor, Channel 2 - left motor
-  try {
-    driver_->GetMotorDriver(LynxMotorNames::LEFT)
-      ->SendCmdVel(roboteq_vel_cmd_converter_.Convert(speed_left));
-    driver_->GetMotorDriver(LynxMotorNames::RIGHT)
-      ->SendCmdVel(roboteq_vel_cmd_converter_.Convert(speed_right));
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error("Front driver send Roboteq cmd failed: " + std::string(e.what()));
-  }
-
-  if (driver_->IsCANError()) {
+  if (speeds.size() != 2) {
     throw std::runtime_error(
-      "CAN error detected on the front driver when trying to write speed commands.");
+      "Invalid speeds vector size. Expected 2, got " + std::to_string(speeds.size()));
   }
-}
 
-void LynxRobotDriver::TurnOnEStop()
-{
-  try {
-    driver_->TurnOnEStop();
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Failed to turn on E-stop on the front driver: " + std::string(e.what()));
-  }
-}
+  const auto speed_left = this->GetCmdVelConverter().Convert(speeds.at(0));
+  const auto speed_right = this->GetCmdVelConverter().Convert(speeds.at(1));
 
-void LynxRobotDriver::TurnOffEStop()
-{
   try {
-    driver_->TurnOffEStop();
+    drivers_.at(DriverNames::DEFAULT)->GetMotorDriver(MotorNames::LEFT)->SendCmdVel(speed_left);
+    drivers_.at(DriverNames::DEFAULT)->GetMotorDriver(MotorNames::RIGHT)->SendCmdVel(speed_right);
   } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Failed to turn off E-stop on the front driver: " + std::string(e.what()));
+    throw std::runtime_error("Driver send Roboteq cmd failed: " + std::string(e.what()));
   }
-}
 
-void LynxRobotDriver::TurnOnSafetyStop()
-{
-  try {
-    driver_->GetMotorDriver(LynxMotorNames::LEFT)->TurnOnSafetyStop();
-    driver_->GetMotorDriver(LynxMotorNames::RIGHT)->TurnOnSafetyStop();
-  } catch (const std::runtime_error & e) {
+  if (drivers_.at(DriverNames::DEFAULT)->IsCANError()) {
     throw std::runtime_error(
-      "Failed to turn on safety stop on the front driver: " + std::string(e.what()));
+      "CAN error detected on the Driver when trying to write speed commands.");
   }
 }
 
-void LynxRobotDriver::DefineDriver()
+void LynxRobotDriver::DefineDrivers()
 {
-  driver_ = std::make_shared<RoboteqDriver>(
-    canopen_manager_.GetMaster(), canopen_settings_.driver_can_ids.at(LynxDriverNames::DEFAULT),
+  auto driver = std::make_shared<RoboteqDriver>(
+    canopen_manager_.GetMaster(), canopen_settings_.driver_can_ids.at(DriverNames::FRONT),
     canopen_settings_.sdo_operation_timeout_ms);
 
   auto left_motor_driver = std::make_shared<RoboteqMotorDriver>(
-    std::dynamic_pointer_cast<RoboteqDriver>(driver_), LynxMotorChannel::LEFT);
+    std::dynamic_pointer_cast<RoboteqDriver>(driver), MotorChannels::LEFT);
   auto right_motor_driver = std::make_shared<RoboteqMotorDriver>(
-    std::dynamic_pointer_cast<RoboteqDriver>(driver_), LynxMotorChannel::RIGHT);
-
-  driver_->AddMotorDriver(LynxMotorNames::LEFT, left_motor_driver);
-  driver_->AddMotorDriver(LynxMotorNames::RIGHT, right_motor_driver);
-}
+    std::dynamic_pointer_cast<RoboteqDriver>(driver), MotorChannels::RIGHT);
 
-void LynxRobotDriver::SetMotorsStates(
-  RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
-  const timespec & current_time)
-{
-  // TODO figure out both motors timestamps
-  bool data_timed_out =
-    (lely::util::from_timespec(current_time) - lely::util::from_timespec(left_state.pos_timestamp) >
-     pdo_motor_states_timeout_ms_) ||
-    (lely::util::from_timespec(current_time) -
-       lely::util::from_timespec(left_state.vel_current_timestamp) >
-     pdo_motor_states_timeout_ms_);
-
-  // Channel 1 - right, Channel 2 - left
-  data.SetMotorsStates(right_state, left_state, data_timed_out);
-}
-
-void LynxRobotDriver::SetDriverState(
-  RoboteqData & data, const DriverState & state, const timespec & current_time)
-{
-  bool data_timed_out = (lely::util::from_timespec(current_time) -
-                           lely::util::from_timespec(state.flags_current_timestamp) >
-                         pdo_driver_state_timeout_ms_) ||
-                        (lely::util::from_timespec(current_time) -
-                           lely::util::from_timespec(state.voltages_temps_timestamp) >
-                         pdo_driver_state_timeout_ms_);
+  driver->AddMotorDriver(MotorNames::LEFT, left_motor_driver);
+  driver->AddMotorDriver(MotorNames::RIGHT, right_motor_driver);
 
-  data.SetDriverState(state, data_timed_out);
+  drivers_.emplace(DriverNames::DEFAULT, driver);
 }
 
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp
index c9e46a57..7cbbb974 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp
@@ -12,301 +12,79 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp"
 
-#include <chrono>
-#include <ctime>
 #include <memory>
 #include <stdexcept>
 #include <string>
-#include <thread>
-#include <utility>
-
-#include "lely/util/chrono.hpp"
+#include <vector>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
 #include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
 
 namespace panther_hardware_interfaces
 {
 
-PantherRobotDriver::PantherRobotDriver(
-  const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings,
-  const std::chrono::milliseconds activate_wait_time)
-: canopen_settings_(canopen_settings),
-  canopen_manager_(canopen_settings),
-  front_data_(drivetrain_settings),
-  rear_data_(drivetrain_settings),
-  roboteq_vel_cmd_converter_(drivetrain_settings),
-  pdo_motor_states_timeout_ms_(canopen_settings.pdo_motor_states_timeout_ms),
-  pdo_driver_state_timeout_ms_(canopen_settings.pdo_driver_state_timeout_ms),
-  activate_wait_time_(activate_wait_time)
-{
-}
-
-void PantherRobotDriver::Initialize()
-{
-  if (initialized_) {
-    return;
-  }
-
-  try {
-    canopen_manager_.Initialize();
-    DefineDrivers();
-    front_driver_->Boot();
-    rear_driver_->Boot();
-  } catch (const std::runtime_error & e) {
-    throw e;
-  }
-
-  initialized_ = true;
-}
-
-void PantherRobotDriver::Deinitialize()
-{
-  canopen_manager_.Deinitialize();
-  initialized_ = false;
-}
-
-void PantherRobotDriver::Activate()
+void PantherRobotDriver::SendSpeedCommands(const std::vector<float> & speeds)
 {
-  // Activation procedure - it is necessary to first reset scripts, wait for a bit (1 second)
-  // and then send 0 commands for some time (also 1 second)
-
-  try {
-    front_driver_->ResetScript();
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Front driver reset Roboteq script exception: " + std::string(e.what()));
-  }
-
-  try {
-    rear_driver_->ResetScript();
-  } catch (const std::runtime_error & e) {
+  if (speeds.size() != 4) {
     throw std::runtime_error(
-      "Rear driver reset Roboteq script exception: " + std::string(e.what()));
-  }
-
-  std::this_thread::sleep_for(activate_wait_time_);
-
-  try {
-    front_driver_->GetMotorDriver(PantherMotorNames::LEFT)->SendCmdVel(0);
-    front_driver_->GetMotorDriver(PantherMotorNames::RIGHT)->SendCmdVel(0);
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error("Front driver send 0 command exception: " + std::string(e.what()));
-  }
-
-  try {
-    rear_driver_->GetMotorDriver(PantherMotorNames::LEFT)->SendCmdVel(0);
-    rear_driver_->GetMotorDriver(PantherMotorNames::RIGHT)->SendCmdVel(0);
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error("Rear driver send 0 command exception: " + std::string(e.what()));
-  }
-
-  std::this_thread::sleep_for(activate_wait_time_);
-}
-
-void PantherRobotDriver::UpdateCommunicationState()
-{
-  front_data_.SetCANError(front_driver_->IsCANError());
-  rear_data_.SetCANError(rear_driver_->IsCANError());
-
-  front_data_.SetHeartbeatTimeout(front_driver_->IsHeartbeatTimeout());
-  rear_data_.SetHeartbeatTimeout(rear_driver_->IsHeartbeatTimeout());
-}
-
-void PantherRobotDriver::UpdateMotorsState()
-{
-  timespec current_time;
-  clock_gettime(CLOCK_MONOTONIC, &current_time);
-
-  const auto fl_state =
-    front_driver_->GetMotorDriver(PantherMotorNames::LEFT)->ReadMotorDriverState();
-  const auto fr_state =
-    front_driver_->GetMotorDriver(PantherMotorNames::RIGHT)->ReadMotorDriverState();
-  const auto rl_state =
-    rear_driver_->GetMotorDriver(PantherMotorNames::LEFT)->ReadMotorDriverState();
-  const auto rr_state =
-    rear_driver_->GetMotorDriver(PantherMotorNames::RIGHT)->ReadMotorDriverState();
-
-  SetMotorsStates(front_data_, fl_state, fr_state, current_time);
-  SetMotorsStates(rear_data_, rl_state, rr_state, current_time);
-
-  UpdateCommunicationState();
-
-  if (front_data_.IsCANError() || rear_data_.IsCANError()) {
-    throw std::runtime_error("CAN error.");
-  }
-
-  if (front_data_.IsHeartbeatTimeout() || rear_data_.IsHeartbeatTimeout()) {
-    throw std::runtime_error("Motor controller heartbeat timeout.");
-  }
-}
-
-void PantherRobotDriver::UpdateDriversState()
-{
-  timespec current_time;
-  clock_gettime(CLOCK_MONOTONIC, &current_time);
-
-  SetDriverState(front_data_, front_driver_->ReadDriverState(), current_time);
-  SetDriverState(rear_data_, rear_driver_->ReadDriverState(), current_time);
-
-  UpdateCommunicationState();
-
-  if (front_data_.IsCANError() || rear_data_.IsCANError()) {
-    throw std::runtime_error("CAN error.");
+      "Invalid speeds vector size. Expected 4, got " + std::to_string(speeds.size()));
   }
 
-  if (front_data_.IsHeartbeatTimeout() || rear_data_.IsHeartbeatTimeout()) {
-    throw std::runtime_error("Motor controller heartbeat timeout.");
-  }
-}
+  const auto speed_fl = this->GetCmdVelConverter().Convert(speeds.at(0));
+  const auto speed_fr = this->GetCmdVelConverter().Convert(speeds.at(1));
+  const auto speed_rl = this->GetCmdVelConverter().Convert(speeds.at(2));
+  const auto speed_rr = this->GetCmdVelConverter().Convert(speeds.at(3));
 
-const RoboteqData & PantherRobotDriver::GetData(const std::string & name)
-{
-  if (name == PantherDriverNames::FRONT) {
-    return front_data_;
-  } else if (name == PantherDriverNames::REAR) {
-    return rear_data_;
-  } else {
-    throw std::runtime_error("Data with name '" + name + "' does not exist.");
-  }
-}
-
-void PantherRobotDriver::SendSpeedCommands(
-  const float speed_fl, const float speed_fr, const float speed_rl, const float speed_rr)
-{
-  // Channel 1 - right motor, Channel 2 - left motor
   try {
-    front_driver_->GetMotorDriver(PantherMotorNames::LEFT)
-      ->SendCmdVel(roboteq_vel_cmd_converter_.Convert(speed_fl));
-    front_driver_->GetMotorDriver(PantherMotorNames::RIGHT)
-      ->SendCmdVel(roboteq_vel_cmd_converter_.Convert(speed_fr));
+    drivers_.at(DriverNames::FRONT)->GetMotorDriver(MotorNames::LEFT)->SendCmdVel(speed_fl);
+    drivers_.at(DriverNames::FRONT)->GetMotorDriver(MotorNames::RIGHT)->SendCmdVel(speed_fr);
   } catch (const std::runtime_error & e) {
     throw std::runtime_error("Front driver send Roboteq cmd failed: " + std::string(e.what()));
   }
   try {
-    rear_driver_->GetMotorDriver(PantherMotorNames::LEFT)
-      ->SendCmdVel(roboteq_vel_cmd_converter_.Convert(speed_rl));
-    rear_driver_->GetMotorDriver(PantherMotorNames::RIGHT)
-      ->SendCmdVel(roboteq_vel_cmd_converter_.Convert(speed_rr));
+    drivers_.at(DriverNames::REAR)->GetMotorDriver(MotorNames::LEFT)->SendCmdVel(speed_rl);
+    drivers_.at(DriverNames::REAR)->GetMotorDriver(MotorNames::RIGHT)->SendCmdVel(speed_rr);
   } catch (const std::runtime_error & e) {
     throw std::runtime_error("Rear driver send Roboteq cmd failed: " + std::string(e.what()));
   }
 
-  if (front_driver_->IsCANError()) {
+  if (drivers_.at(DriverNames::FRONT)->IsCANError()) {
     throw std::runtime_error(
       "CAN error detected on the front driver when trying to write speed commands.");
   }
-  if (rear_driver_->IsCANError()) {
+  if (drivers_.at(DriverNames::REAR)->IsCANError()) {
     throw std::runtime_error(
       "CAN error detected on the rear driver when trying to write speed commands.");
   }
 }
 
-void PantherRobotDriver::TurnOnEStop()
-{
-  try {
-    front_driver_->TurnOnEStop();
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Failed to turn on E-stop on the front driver: " + std::string(e.what()));
-  }
-  try {
-    rear_driver_->TurnOnEStop();
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Failed to turn on E-stop on the rear driver: " + std::string(e.what()));
-  }
-}
-
-void PantherRobotDriver::TurnOffEStop()
-{
-  try {
-    front_driver_->TurnOffEStop();
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Failed to turn off E-stop on the front driver: " + std::string(e.what()));
-  }
-  try {
-    rear_driver_->TurnOffEStop();
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Failed to turn off E-stop on the rear driver: " + std::string(e.what()));
-  }
-}
-
-void PantherRobotDriver::TurnOnSafetyStop()
-{
-  try {
-    front_driver_->GetMotorDriver(PantherMotorNames::LEFT)->TurnOnSafetyStop();
-    front_driver_->GetMotorDriver(PantherMotorNames::RIGHT)->TurnOnSafetyStop();
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Failed to turn on safety stop on the front driver: " + std::string(e.what()));
-  }
-  try {
-    rear_driver_->GetMotorDriver(PantherMotorNames::LEFT)->TurnOnSafetyStop();
-    rear_driver_->GetMotorDriver(PantherMotorNames::RIGHT)->TurnOnSafetyStop();
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Failed to turn on safety stop on the rear driver: " + std::string(e.what()));
-  }
-}
-
 void PantherRobotDriver::DefineDrivers()
 {
-  front_driver_ = std::make_shared<RoboteqDriver>(
-    canopen_manager_.GetMaster(), canopen_settings_.driver_can_ids.at(PantherDriverNames::FRONT),
+  auto front_driver = std::make_shared<RoboteqDriver>(
+    canopen_manager_.GetMaster(), canopen_settings_.driver_can_ids.at(DriverNames::FRONT),
     canopen_settings_.sdo_operation_timeout_ms);
-  rear_driver_ = std::make_shared<RoboteqDriver>(
-    canopen_manager_.GetMaster(), canopen_settings_.driver_can_ids.at(PantherDriverNames::REAR),
+  auto rear_driver = std::make_shared<RoboteqDriver>(
+    canopen_manager_.GetMaster(), canopen_settings_.driver_can_ids.at(DriverNames::REAR),
     canopen_settings_.sdo_operation_timeout_ms);
 
   auto fl_motor_driver = std::make_shared<RoboteqMotorDriver>(
-    std::dynamic_pointer_cast<RoboteqDriver>(front_driver_), PantherMotorChannel::LEFT);
+    std::dynamic_pointer_cast<RoboteqDriver>(front_driver), MotorChannels::LEFT);
   auto fr_motor_driver = std::make_shared<RoboteqMotorDriver>(
-    std::dynamic_pointer_cast<RoboteqDriver>(front_driver_), PantherMotorChannel::RIGHT);
+    std::dynamic_pointer_cast<RoboteqDriver>(front_driver), MotorChannels::RIGHT);
   auto rl_motor_driver = std::make_shared<RoboteqMotorDriver>(
-    std::dynamic_pointer_cast<RoboteqDriver>(rear_driver_), PantherMotorChannel::LEFT);
+    std::dynamic_pointer_cast<RoboteqDriver>(rear_driver), MotorChannels::LEFT);
   auto rr_motor_driver = std::make_shared<RoboteqMotorDriver>(
-    std::dynamic_pointer_cast<RoboteqDriver>(rear_driver_), PantherMotorChannel::RIGHT);
+    std::dynamic_pointer_cast<RoboteqDriver>(rear_driver), MotorChannels::RIGHT);
 
-  front_driver_->AddMotorDriver(PantherMotorNames::LEFT, fl_motor_driver);
-  front_driver_->AddMotorDriver(PantherMotorNames::RIGHT, fr_motor_driver);
-  rear_driver_->AddMotorDriver(PantherMotorNames::LEFT, rl_motor_driver);
-  rear_driver_->AddMotorDriver(PantherMotorNames::RIGHT, rr_motor_driver);
-}
-
-void PantherRobotDriver::SetMotorsStates(
-  RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
-  const timespec & current_time)
-{
-  // TODO figure out both motors timestamps
-  bool data_timed_out =
-    (lely::util::from_timespec(current_time) - lely::util::from_timespec(left_state.pos_timestamp) >
-     pdo_motor_states_timeout_ms_) ||
-    (lely::util::from_timespec(current_time) -
-       lely::util::from_timespec(left_state.vel_current_timestamp) >
-     pdo_motor_states_timeout_ms_);
-
-  // Channel 1 - right, Channel 2 - left
-  data.SetMotorsStates(right_state, left_state, data_timed_out);
-}
-
-void PantherRobotDriver::SetDriverState(
-  RoboteqData & data, const DriverState & state, const timespec & current_time)
-{
-  bool data_timed_out = (lely::util::from_timespec(current_time) -
-                           lely::util::from_timespec(state.flags_current_timestamp) >
-                         pdo_driver_state_timeout_ms_) ||
-                        (lely::util::from_timespec(current_time) -
-                           lely::util::from_timespec(state.voltages_temps_timestamp) >
-                         pdo_driver_state_timeout_ms_);
+  front_driver->AddMotorDriver(MotorNames::LEFT, fl_motor_driver);
+  front_driver->AddMotorDriver(MotorNames::RIGHT, fr_motor_driver);
+  rear_driver->AddMotorDriver(MotorNames::LEFT, rl_motor_driver);
+  rear_driver->AddMotorDriver(MotorNames::RIGHT, rr_motor_driver);
 
-  data.SetDriverState(state, data_timed_out);
+  drivers_.emplace(DriverNames::FRONT, front_driver);
+  drivers_.emplace(DriverNames::REAR, rear_driver);
 }
 
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
new file mode 100644
index 00000000..f0eaca37
--- /dev/null
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
@@ -0,0 +1,235 @@
+// 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_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
+
+#include <chrono>
+#include <ctime>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <thread>
+#include <utility>
+
+#include "lely/util/chrono.hpp"
+
+#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
+
+namespace panther_hardware_interfaces
+{
+
+RoboteqRobotDriver::RoboteqRobotDriver(
+  const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings,
+  const std::chrono::milliseconds activate_wait_time)
+: canopen_settings_(canopen_settings),
+  drivetrain_settings_(drivetrain_settings),
+  canopen_manager_(canopen_settings),
+  roboteq_vel_cmd_converter_(drivetrain_settings),
+  pdo_motor_states_timeout_ms_(canopen_settings.pdo_motor_states_timeout_ms),
+  pdo_driver_state_timeout_ms_(canopen_settings.pdo_driver_state_timeout_ms),
+  activate_wait_time_(activate_wait_time)
+{
+}
+
+void RoboteqRobotDriver::Initialize()
+{
+  if (initialized_) {
+    return;
+  }
+
+  try {
+    canopen_manager_.Initialize();
+    DefineDrivers();
+    for (auto & [name, driver] : drivers_) {
+      data_.emplace(name, RoboteqData(drivetrain_settings_));
+      driver->Boot();
+    }
+  } catch (const std::runtime_error & e) {
+    throw e;
+  }
+
+  initialized_ = true;
+}
+
+void RoboteqRobotDriver::Deinitialize()
+{
+  canopen_manager_.Deinitialize();
+  initialized_ = false;
+}
+
+void RoboteqRobotDriver::Activate()
+{
+  // Activation procedure - it is necessary to first reset scripts, wait for a bit (1 second)
+  // and then send 0 commands for some time (also 1 second)
+
+  for (auto & [name, driver] : drivers_) {
+    try {
+      driver->ResetScript();
+    } catch (const std::runtime_error & e) {
+      throw std::runtime_error(
+        "Reset Roboteq script exception on " + name + "driver : " + std::string(e.what()));
+    }
+  }
+
+  std::this_thread::sleep_for(activate_wait_time_);
+
+  for (auto & [name, driver] : drivers_) {
+    try {
+      driver->GetMotorDriver(MotorNames::LEFT)->SendCmdVel(0);
+      driver->GetMotorDriver(MotorNames::RIGHT)->SendCmdVel(0);
+    } catch (const std::runtime_error & e) {
+      throw std::runtime_error(
+        "Send 0 command exception on " + name + "driver : " + std::string(e.what()));
+    }
+  }
+
+  std::this_thread::sleep_for(activate_wait_time_);
+}
+
+void RoboteqRobotDriver::UpdateCommunicationState()
+{
+  for (auto & [name, driver] : drivers_) {
+    data_.at(name).SetCANError(driver->IsCANError());
+    data_.at(name).SetHeartbeatTimeout(driver->IsHeartbeatTimeout());
+  }
+}
+
+void RoboteqRobotDriver::UpdateMotorsState()
+{
+  timespec current_time;
+  clock_gettime(CLOCK_MONOTONIC, &current_time);
+
+  for (auto & [name, driver] : drivers_) {
+    const auto left_state = driver->GetMotorDriver(MotorNames::LEFT)->ReadMotorDriverState();
+    const auto right_state = driver->GetMotorDriver(MotorNames::RIGHT)->ReadMotorDriverState();
+
+    SetMotorsStates(data_.at(name), left_state, right_state, current_time);
+  }
+
+  UpdateCommunicationState();
+
+  if (std::any_of(
+        data_.begin(), data_.end(), [](const auto & data) { return data.second.IsCANError(); })) {
+    throw std::runtime_error("CAN error.");
+  }
+
+  if (std::any_of(data_.begin(), data_.end(), [](const auto & data) {
+        return data.second.IsHeartbeatTimeout();
+      })) {
+    throw std::runtime_error("Motor controller heartbeat timeout.");
+  }
+}
+
+void RoboteqRobotDriver::UpdateDriversState()
+{
+  timespec current_time;
+  clock_gettime(CLOCK_MONOTONIC, &current_time);
+
+  for (auto & [name, driver] : drivers_) {
+    SetDriverState(data_.at(name), driver->ReadDriverState(), current_time);
+  }
+
+  UpdateCommunicationState();
+
+  if (std::any_of(
+        data_.begin(), data_.end(), [](const auto & data) { return data.second.IsCANError(); })) {
+    throw std::runtime_error("CAN error.");
+  }
+
+  if (std::any_of(data_.begin(), data_.end(), [](const auto & data) {
+        return data.second.IsHeartbeatTimeout();
+      })) {
+    throw std::runtime_error("Motor controller heartbeat timeout.");
+  }
+}
+
+const RoboteqData & RoboteqRobotDriver::GetData(const std::string & name)
+{
+  if (data_.find(name) == data_.end()) {
+    throw std::runtime_error("Data with name '" + name + "' does not exist.");
+  }
+
+  return data_.at(name);
+}
+
+void RoboteqRobotDriver::TurnOnEStop()
+{
+  for (auto & [name, driver] : drivers_) {
+    try {
+      driver->TurnOnEStop();
+    } catch (const std::runtime_error & e) {
+      throw std::runtime_error(
+        "Failed to turn on E-stop on " + name + " driver: " + std::string(e.what()));
+    }
+  }
+}
+
+void RoboteqRobotDriver::TurnOffEStop()
+{
+  for (auto & [name, driver] : drivers_) {
+    try {
+      driver->TurnOffEStop();
+    } catch (const std::runtime_error & e) {
+      throw std::runtime_error(
+        "Failed to turn off E-stop on " + name + " driver: " + std::string(e.what()));
+    }
+  }
+}
+
+void RoboteqRobotDriver::TurnOnSafetyStop()
+{
+  for (auto & [name, driver] : drivers_) {
+    try {
+      driver->GetMotorDriver(MotorNames::LEFT)->TurnOnSafetyStop();
+      driver->GetMotorDriver(MotorNames::RIGHT)->TurnOnSafetyStop();
+    } catch (const std::runtime_error & e) {
+      throw std::runtime_error(
+        "Failed to turn on safety stop on " + name + " driver: " + std::string(e.what()));
+    }
+  }
+}
+
+void RoboteqRobotDriver::SetMotorsStates(
+  RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
+  const timespec & current_time)
+{
+  // TODO figure out both motors timestamps
+  bool data_timed_out =
+    (lely::util::from_timespec(current_time) - lely::util::from_timespec(left_state.pos_timestamp) >
+     pdo_motor_states_timeout_ms_) ||
+    (lely::util::from_timespec(current_time) -
+       lely::util::from_timespec(left_state.vel_current_timestamp) >
+     pdo_motor_states_timeout_ms_);
+
+  // Channel 1 - right, Channel 2 - left
+  data.SetMotorsStates(right_state, left_state, data_timed_out);
+}
+
+void RoboteqRobotDriver::SetDriverState(
+  RoboteqData & data, const DriverState & state, const timespec & current_time)
+{
+  bool data_timed_out = (lely::util::from_timespec(current_time) -
+                           lely::util::from_timespec(state.flags_current_timestamp) >
+                         pdo_driver_state_timeout_ms_) ||
+                        (lely::util::from_timespec(current_time) -
+                           lely::util::from_timespec(state.voltages_temps_timestamp) >
+                         pdo_driver_state_timeout_ms_);
+
+  data.SetDriverState(state, data_timed_out);
+}
+
+}  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
index 584a4c00..21c6dfc1 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
@@ -14,11 +14,9 @@
 
 #include <chrono>
 #include <cstdint>
-#include <future>
 #include <memory>
 #include <stdexcept>
-#include <string>
-#include <thread>
+#include <vector>
 
 #include <gmock/gmock.h>
 
@@ -31,6 +29,8 @@
 #include "utils/mock_driver.hpp"
 #include "utils/test_constants.hpp"
 
+#include "panther_utils/test/test_utils.hpp"
+
 class LynxRobotDriverWrapper : public panther_hardware_interfaces::LynxRobotDriver
 {
 public:
@@ -47,11 +47,16 @@ class LynxRobotDriverWrapper : public panther_hardware_interfaces::LynxRobotDriv
 
     mock_driver_ =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
-    mock_driver_->AddMotorDriver(kLeftMotorDriverName, mock_left_motor_driver_);
-    mock_driver_->AddMotorDriver(kRightMotorDriverName, mock_right_motor_driver_);
+    mock_driver_->AddMotorDriver(
+      panther_hardware_interfaces::MotorNames::LEFT, mock_left_motor_driver_);
+    mock_driver_->AddMotorDriver(
+      panther_hardware_interfaces::MotorNames::RIGHT, mock_right_motor_driver_);
   }
 
-  void DefineDriver() override { driver_ = mock_driver_; }
+  void DefineDrivers() override
+  {
+    drivers_.emplace(panther_hardware_interfaces::DriverNames::DEFAULT, mock_driver_);
+  }
 
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>> GetMockDriver()
   {
@@ -70,10 +75,6 @@ class LynxRobotDriverWrapper : public panther_hardware_interfaces::LynxRobotDriv
     return mock_right_motor_driver_;
   }
 
-  static constexpr char kDriverName[] = "default";
-  static constexpr char kLeftMotorDriverName[] = "left";
-  static constexpr char kRightMotorDriverName[] = "right";
-
 private:
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>> mock_driver_;
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
@@ -82,10 +83,10 @@ class LynxRobotDriverWrapper : public panther_hardware_interfaces::LynxRobotDriv
     mock_right_motor_driver_;
 };
 
-class TestLynxRobotDriverInitialization : public ::testing::Test
+class TestLynxRobotDriver : public ::testing::Test
 {
 public:
-  TestLynxRobotDriverInitialization()
+  TestLynxRobotDriver()
   {
     can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
       panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
@@ -94,320 +95,68 @@ class TestLynxRobotDriverInitialization : public ::testing::Test
     robot_driver_ = std::make_unique<LynxRobotDriverWrapper>(
       panther_hardware_interfaces_test::kCANopenSettings,
       panther_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10));
-  }
-
-  ~TestLynxRobotDriverInitialization() {}
 
-protected:
-  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
-  std::unique_ptr<LynxRobotDriverWrapper> robot_driver_;
-};
-
-class TestLynxRobotDriver : public TestLynxRobotDriverInitialization
-{
-public:
-  TestLynxRobotDriver()
-  {
     robot_driver_->Initialize();
     robot_driver_->Activate();
   }
 
   ~TestLynxRobotDriver() { robot_driver_->Deinitialize(); }
-};
-
-TEST_F(TestLynxRobotDriverInitialization, Initialize)
-{
-  EXPECT_CALL(*robot_driver_->GetMockDriver(), Boot()).Times(1);
-
-  EXPECT_NO_THROW(robot_driver_->Initialize());
-  ASSERT_NO_THROW(robot_driver_->Deinitialize());
-
-  // Check if deinitialization worked correctly - initialize once again
-  EXPECT_CALL(*robot_driver_->GetMockDriver(), Boot()).Times(1);
-  EXPECT_NO_THROW(robot_driver_->Initialize());
-}
-
-TEST_F(TestLynxRobotDriverInitialization, Activate)
-{
-  EXPECT_CALL(*robot_driver_->GetMockDriver(), ResetScript()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockLeftMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRightMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
-
-  ASSERT_NO_THROW(robot_driver_->Initialize());
-  EXPECT_NO_THROW(robot_driver_->Activate());
-}
-
-TEST_F(TestLynxRobotDriver, UpdateCommunicationState)
-{
-  EXPECT_CALL(*robot_driver_->GetMockDriver(), IsCANError()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockDriver(), IsHeartbeatTimeout()).Times(1);
-
-  EXPECT_NO_THROW(robot_driver_->UpdateCommunicationState());
-}
-
-TEST_F(TestLynxRobotDriver, UpdateCommunicationStateCANErorr)
-{
-  EXPECT_CALL(*robot_driver_->GetMockDriver(), IsCANError()).WillOnce(::testing::Return(true));
-
-  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
-
-  EXPECT_TRUE(robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsCANError());
-}
-
-TEST_F(TestLynxRobotDriver, UpdateCommunicationStateHeartbeatTimeout)
-{
-  EXPECT_CALL(*robot_driver_->GetMockDriver(), IsHeartbeatTimeout())
-    .WillOnce(::testing::Return(true));
-
-  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
-
-  EXPECT_TRUE(robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsHeartbeatTimeout());
-}
-
-TEST_F(TestLynxRobotDriver, UpdateMotorsState)
-{
-  using panther_hardware_interfaces::LynxMotorChannel;
-  using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
-  using panther_hardware_interfaces_test::kRbtqPosFbToRad;
-  using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
-
-  const std::int32_t l_pos = 101;
-  const std::int32_t l_vel = 102;
-  const std::int32_t l_current = 103;
-  const std::int32_t r_pos = 201;
-  const std::int32_t r_vel = 202;
-  const std::int32_t r_current = 203;
-
-  ON_CALL(*robot_driver_->GetMockLeftMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Return(
-      panther_hardware_interfaces::MotorDriverState({l_pos, l_vel, l_current, {0, 0}, {0, 0}})));
-  ON_CALL(*robot_driver_->GetMockRightMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Return(
-      panther_hardware_interfaces::MotorDriverState({r_pos, r_vel, r_current, {0, 0}, {0, 0}})));
-
-  robot_driver_->UpdateMotorsState();
-
-  const auto & left_data = robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName)
-                             .GetMotorState(LynxMotorChannel::LEFT);
-  const auto & right_data = robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName)
-                              .GetMotorState(LynxMotorChannel::RIGHT);
-
-  EXPECT_FLOAT_EQ(left_data.GetPosition(), l_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(left_data.GetVelocity(), l_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(left_data.GetTorque(), l_current * kRbtqCurrentFbToNewtonMeters);
-
-  EXPECT_FLOAT_EQ(right_data.GetPosition(), r_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(right_data.GetVelocity(), r_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(right_data.GetTorque(), r_current * kRbtqCurrentFbToNewtonMeters);
-}
-
-TEST_F(TestLynxRobotDriver, UpdateMotorsStateTimestamps)
-{
-  auto read_motor_driver_state_method = []() {
-    panther_hardware_interfaces::MotorDriverState state;
-    clock_gettime(CLOCK_MONOTONIC, &state.pos_timestamp);
-    clock_gettime(CLOCK_MONOTONIC, &state.vel_current_timestamp);
-    return state;
-  };
-
-  ON_CALL(*robot_driver_->GetMockLeftMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
-  ON_CALL(*robot_driver_->GetMockRightMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
 
-  robot_driver_->UpdateMotorsState();
-
-  // sleep for timeout and check if timestamps were updated correctly
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
-    std::chrono::milliseconds(10));
-
-  robot_driver_->UpdateMotorsState();
-
-  EXPECT_FALSE(
-    robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsMotorStatesDataTimedOut());
-}
-
-TEST_F(TestLynxRobotDriver, UpdateMotorsStateTimeout)
-{
-  timespec current_time;
-  clock_gettime(CLOCK_MONOTONIC, &current_time);
-
-  panther_hardware_interfaces::MotorDriverState state = {0, 0, 0, current_time, current_time};
-
-  ON_CALL(*robot_driver_->GetMockLeftMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Return(state));
-  ON_CALL(*robot_driver_->GetMockRightMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Return(state));
-
-  // sleep for pdo_motor_states_timeout_ms + 10ms
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
-    std::chrono::milliseconds(10));
-
-  robot_driver_->UpdateMotorsState();
-
-  EXPECT_TRUE(
-    robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsMotorStatesDataTimedOut());
-  EXPECT_TRUE(robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsError());
-}
-
-TEST_F(TestLynxRobotDriver, UpdateDriverState)
-{
-  using panther_hardware_interfaces::LynxMotorChannel;
-
-  const std::int16_t f_temp = 30;
-  const std::int16_t f_heatsink_temp = 31;
-  const std::uint16_t f_volt = 400;
-  const std::int16_t f_battery_current_1 = 10;
-  const std::int16_t f_battery_current_2 = 30;
-
-  const std::uint8_t fault_flag_overheat = static_cast<std::uint8_t>(0b01);
-  const std::uint8_t script_flag_encoder_disconnected = static_cast<std::uint8_t>(0b10);
-  const std::uint8_t runtime_error_loop_error = static_cast<std::uint8_t>(0b100);
-  const std::uint8_t runtime_error_safety_stop_active = static_cast<std::uint8_t>(0b1000);
-
-  ON_CALL(*robot_driver_->GetMockDriver(), ReadDriverState())
-    .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
-      {fault_flag_overheat,
-       script_flag_encoder_disconnected,
-       runtime_error_loop_error,
-       runtime_error_safety_stop_active,
-       f_battery_current_1,
-       f_battery_current_2,
-       f_volt,
-       f_temp,
-       f_heatsink_temp,
-       {0, 0},
-       {0, 0}})));
-
-  robot_driver_->UpdateDriversState();
-
-  const auto & data = robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName);
-  const auto & driver_state =
-    robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).GetDriverState();
-
-  EXPECT_EQ(static_cast<std::int16_t>(driver_state.GetTemperature()), f_temp);
-  EXPECT_EQ(static_cast<std::int16_t>(driver_state.GetHeatsinkTemperature()), f_heatsink_temp);
-  EXPECT_EQ(static_cast<std::uint16_t>(driver_state.GetVoltage() * 10.0), f_volt);
-  EXPECT_EQ(
-    static_cast<std::int16_t>(driver_state.GetCurrent() * 10.0),
-    f_battery_current_1 + f_battery_current_2);
-
-  EXPECT_TRUE(data.GetFaultFlag().GetMessage().overheat);
-  EXPECT_TRUE(data.GetScriptFlag().GetMessage().encoder_disconnected);
-  EXPECT_TRUE(data.GetRuntimeError(LynxMotorChannel::RIGHT).GetMessage().loop_error);
-  EXPECT_TRUE(data.GetRuntimeError(LynxMotorChannel::LEFT).GetMessage().safety_stop_active);
-}
-
-TEST_F(TestLynxRobotDriver, UpdateDriverStateTimestamps)
-{
-  auto read_driver_state_method = []() {
-    panther_hardware_interfaces::DriverState state;
-    clock_gettime(CLOCK_MONOTONIC, &state.flags_current_timestamp);
-    clock_gettime(CLOCK_MONOTONIC, &state.voltages_temps_timestamp);
-    return state;
-  };
-
-  ON_CALL(*robot_driver_->GetMockDriver(), ReadDriverState())
-    .WillByDefault(::testing::Invoke(read_driver_state_method));
-
-  robot_driver_->UpdateDriversState();
-
-  // sleep for timeout and check if timestamps were updated correctly
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
-    std::chrono::milliseconds(10));
-
-  robot_driver_->UpdateDriversState();
-
-  EXPECT_FALSE(
-    robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsDriverStateDataTimedOut());
-}
-
-TEST_F(TestLynxRobotDriver, UpdateDriverStateTimeout)
-{
-  timespec current_time;
-  clock_gettime(CLOCK_MONOTONIC, &current_time);
-
-  panther_hardware_interfaces::DriverState state = {
-    0, 0, 0, 0, 0, 0, 0, 0, 0, current_time, current_time};
-
-  ON_CALL(*robot_driver_->GetMockDriver(), ReadDriverState())
-    .WillByDefault(::testing::Return(state));
-
-  // sleep for pdo_driver_state_timeout_ms + 10ms
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
-    std::chrono::milliseconds(10));
-
-  robot_driver_->UpdateDriversState();
-
-  EXPECT_TRUE(
-    robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsDriverStateDataTimedOut());
-  EXPECT_TRUE(robot_driver_->GetData(LynxRobotDriverWrapper::kDriverName).IsError());
-}
+protected:
+  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
+  std::unique_ptr<LynxRobotDriverWrapper> robot_driver_;
+};
 
 TEST_F(TestLynxRobotDriver, SendSpeedCommands)
 {
   using panther_hardware_interfaces_test::kRadPerSecToRbtqCmd;
 
-  const float l_v = 0.1;
-  const float r_v = 0.2;
+  const float left_v = 0.1;
+  const float right_v = 0.2;
 
+  EXPECT_CALL(*robot_driver_->GetMockDriver(), IsCANError()).Times(1);
   EXPECT_CALL(
     *robot_driver_->GetMockLeftMotorDriver(),
-    SendCmdVel(::testing::Eq(static_cast<std::int32_t>(l_v * kRadPerSecToRbtqCmd))))
+    SendCmdVel(::testing::Eq(static_cast<std::int32_t>(left_v * kRadPerSecToRbtqCmd))))
     .Times(1);
   EXPECT_CALL(
     *robot_driver_->GetMockRightMotorDriver(),
-    SendCmdVel(::testing::Eq(static_cast<std::int32_t>(r_v * kRadPerSecToRbtqCmd))))
+    SendCmdVel(::testing::Eq(static_cast<std::int32_t>(right_v * kRadPerSecToRbtqCmd))))
     .Times(1);
 
-  EXPECT_NO_THROW(robot_driver_->SendSpeedCommands(l_v, r_v, 0, 0));
+  const std::vector<float> speeds = {left_v, right_v};
+  EXPECT_NO_THROW(robot_driver_->SendSpeedCommands(speeds));
 }
 
-TEST_F(TestLynxRobotDriver, TurnOnEStop)
+TEST_F(TestLynxRobotDriver, SendSpeedCommandsSendCmdVelError)
 {
-  EXPECT_CALL(*robot_driver_->GetMockDriver(), TurnOnEStop()).Times(1);
-  EXPECT_NO_THROW(robot_driver_->TurnOnEStop());
-}
+  EXPECT_CALL(*robot_driver_->GetMockLeftMotorDriver(), SendCmdVel(::testing::_))
+    .WillOnce(::testing::Throw(std::runtime_error("")));
 
-TEST_F(TestLynxRobotDriver, TurnOffEStop)
-{
-  EXPECT_CALL(*robot_driver_->GetMockDriver(), TurnOffEStop()).Times(1);
-  EXPECT_NO_THROW(robot_driver_->TurnOffEStop());
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { robot_driver_->SendSpeedCommands({0.0, 0.0}); }, "Driver send Roboteq cmd failed:"));
 }
 
-TEST_F(TestLynxRobotDriver, TurnOnEStopError)
+TEST_F(TestLynxRobotDriver, SendSpeedCommandsCANError)
 {
-  EXPECT_CALL(*robot_driver_->GetMockDriver(), TurnOnEStop())
-    .WillOnce(::testing::Throw(std::runtime_error("")));
-  EXPECT_THROW(robot_driver_->TurnOnEStop(), std::runtime_error);
-}
+  EXPECT_CALL(*robot_driver_->GetMockDriver(), IsCANError()).WillOnce(::testing::Return(true));
 
-TEST_F(TestLynxRobotDriver, TurnOffEStopError)
-{
-  EXPECT_CALL(*robot_driver_->GetMockDriver(), TurnOffEStop())
-    .WillOnce(::testing::Throw(std::runtime_error("")));
-  EXPECT_THROW(robot_driver_->TurnOffEStop(), std::runtime_error);
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { robot_driver_->SendSpeedCommands({0.0, 0.0}); },
+    "CAN error detected on the Driver when trying to write speed commands."));
 }
 
-TEST_F(TestLynxRobotDriver, SafetyStop)
+TEST_F(TestLynxRobotDriver, SendSpeedCommandsInvalidVectorSize)
 {
-  EXPECT_CALL(*robot_driver_->GetMockLeftMotorDriver(), TurnOnSafetyStop()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRightMotorDriver(), TurnOnSafetyStop()).Times(1);
+  const std::vector<float> speeds = {0.1, 0.2, 0.3};
 
-  EXPECT_NO_THROW(robot_driver_->TurnOnSafetyStop());
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { robot_driver_->SendSpeedCommands(speeds); }, "Invalid speeds vector size"));
 }
 
-TEST_F(TestLynxRobotDriver, SafetyStopError)
+TEST_F(TestLynxRobotDriver, AttemptErrorFlagResetWithZeroSpeed)
 {
-  EXPECT_CALL(*robot_driver_->GetMockLeftMotorDriver(), TurnOnSafetyStop())
-    .WillOnce(::testing::Throw(std::runtime_error("")));
-  EXPECT_CALL(*robot_driver_->GetMockRightMotorDriver(), TurnOnSafetyStop()).Times(0);
-
-  EXPECT_THROW(robot_driver_->TurnOnSafetyStop(), std::runtime_error);
+  EXPECT_NO_THROW(robot_driver_->AttemptErrorFlagResetWithZeroSpeed());
 }
 
 int main(int argc, char ** argv)
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
index 1ef15d28..bdd9c5ca 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
@@ -14,11 +14,9 @@
 
 #include <chrono>
 #include <cstdint>
-#include <future>
 #include <memory>
 #include <stdexcept>
-#include <string>
-#include <thread>
+#include <vector>
 
 #include <gmock/gmock.h>
 
@@ -31,6 +29,8 @@
 #include "utils/mock_driver.hpp"
 #include "utils/test_constants.hpp"
 
+#include "panther_utils/test/test_utils.hpp"
+
 class PantherRobotDriverWrapper : public panther_hardware_interfaces::PantherRobotDriver
 {
 public:
@@ -51,19 +51,23 @@ class PantherRobotDriverWrapper : public panther_hardware_interfaces::PantherRob
 
     mock_front_driver_ =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
-    mock_front_driver_->AddMotorDriver(kLeftMotorDriverName, mock_fl_motor_driver_);
-    mock_front_driver_->AddMotorDriver(kRightMotorDriverName, mock_fr_motor_driver_);
+    mock_front_driver_->AddMotorDriver(
+      panther_hardware_interfaces::MotorNames::LEFT, mock_fl_motor_driver_);
+    mock_front_driver_->AddMotorDriver(
+      panther_hardware_interfaces::MotorNames::RIGHT, mock_fr_motor_driver_);
 
     mock_rear_driver_ =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
-    mock_rear_driver_->AddMotorDriver(kLeftMotorDriverName, mock_rl_motor_driver_);
-    mock_rear_driver_->AddMotorDriver(kRightMotorDriverName, mock_rr_motor_driver_);
+    mock_rear_driver_->AddMotorDriver(
+      panther_hardware_interfaces::MotorNames::LEFT, mock_rl_motor_driver_);
+    mock_rear_driver_->AddMotorDriver(
+      panther_hardware_interfaces::MotorNames::RIGHT, mock_rr_motor_driver_);
   }
 
   void DefineDrivers() override
   {
-    front_driver_ = mock_front_driver_;
-    rear_driver_ = mock_rear_driver_;
+    drivers_.emplace(panther_hardware_interfaces::DriverNames::FRONT, mock_front_driver_);
+    drivers_.emplace(panther_hardware_interfaces::DriverNames::REAR, mock_rear_driver_);
   }
 
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
@@ -102,11 +106,6 @@ class PantherRobotDriverWrapper : public panther_hardware_interfaces::PantherRob
     return mock_rr_motor_driver_;
   }
 
-  static constexpr char kFrontDriverName[] = "front";
-  static constexpr char kRearDriverName[] = "rear";
-  static constexpr char kLeftMotorDriverName[] = "left";
-  static constexpr char kRightMotorDriverName[] = "right";
-
 private:
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
     mock_front_driver_;
@@ -122,10 +121,10 @@ class PantherRobotDriverWrapper : public panther_hardware_interfaces::PantherRob
     mock_rr_motor_driver_;
 };
 
-class TestPantherRobotDriverInitialization : public ::testing::Test
+class TestPantherRobotDriver : public ::testing::Test
 {
 public:
-  TestPantherRobotDriverInitialization()
+  TestPantherRobotDriver()
   {
     can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
       panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
@@ -134,360 +133,17 @@ class TestPantherRobotDriverInitialization : public ::testing::Test
     robot_driver_ = std::make_unique<PantherRobotDriverWrapper>(
       panther_hardware_interfaces_test::kCANopenSettings,
       panther_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10));
-  }
-
-  ~TestPantherRobotDriverInitialization() {}
-
-protected:
-  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
-  std::unique_ptr<PantherRobotDriverWrapper> robot_driver_;
-};
 
-class TestPantherRobotDriver : public TestPantherRobotDriverInitialization
-{
-public:
-  TestPantherRobotDriver()
-  {
     robot_driver_->Initialize();
     robot_driver_->Activate();
   }
 
   ~TestPantherRobotDriver() { robot_driver_->Deinitialize(); }
-};
-
-TEST_F(TestPantherRobotDriverInitialization, Initialize)
-{
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), Boot()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), Boot()).Times(1);
-
-  EXPECT_NO_THROW(robot_driver_->Initialize());
-  ASSERT_NO_THROW(robot_driver_->Deinitialize());
-
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), Boot()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), Boot()).Times(1);
-  // Check if deinitialization worked correctly - initialize once again
-  EXPECT_NO_THROW(robot_driver_->Initialize());
-}
-
-TEST_F(TestPantherRobotDriverInitialization, Activate)
-{
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), ResetScript()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), ResetScript()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockFLMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockFRMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRLMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRRMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
-
-  ASSERT_NO_THROW(robot_driver_->Initialize());
-  ASSERT_NO_THROW(robot_driver_->Activate());
-}
-
-TEST_F(TestPantherRobotDriver, UpdateCommunicationState)
-{
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsCANError()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsHeartbeatTimeout()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsCANError()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsHeartbeatTimeout()).Times(1);
-
-  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
-}
-
-TEST_F(TestPantherRobotDriver, UpdateCommunicationStateCANErorr)
-{
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsCANError()).WillOnce(::testing::Return(true));
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsCANError()).WillOnce(::testing::Return(true));
-
-  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
-
-  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName).IsCANError());
-  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsCANError());
-}
-
-TEST_F(TestPantherRobotDriver, UpdateCommunicationStateHeartbeatTimeout)
-{
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsHeartbeatTimeout())
-    .WillOnce(::testing::Return(true));
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsHeartbeatTimeout())
-    .WillOnce(::testing::Return(true));
-
-  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
 
-  EXPECT_TRUE(
-    robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName).IsHeartbeatTimeout());
-  EXPECT_TRUE(
-    robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsHeartbeatTimeout());
-}
-
-TEST_F(TestPantherRobotDriver, UpdateMotorsState)
-{
-  using panther_hardware_interfaces::PantherMotorChannel;
-  using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
-  using panther_hardware_interfaces_test::kRbtqPosFbToRad;
-  using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
-
-  const std::int32_t fl_pos = 101;
-  const std::int32_t fl_vel = 102;
-  const std::int32_t fl_current = 103;
-  const std::int32_t fr_pos = 201;
-  const std::int32_t fr_vel = 202;
-  const std::int32_t fr_current = 203;
-  const std::int32_t rl_pos = 301;
-  const std::int32_t rl_vel = 302;
-  const std::int32_t rl_current = 303;
-  const std::int32_t rr_pos = 401;
-  const std::int32_t rr_vel = 402;
-  const std::int32_t rr_current = 403;
-
-  ON_CALL(*robot_driver_->GetMockFLMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Return(
-      panther_hardware_interfaces::MotorDriverState({fl_pos, fl_vel, fl_current, {0, 0}, {0, 0}})));
-  ON_CALL(*robot_driver_->GetMockFRMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Return(
-      panther_hardware_interfaces::MotorDriverState({fr_pos, fr_vel, fr_current, {0, 0}, {0, 0}})));
-  ON_CALL(*robot_driver_->GetMockRLMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Return(
-      panther_hardware_interfaces::MotorDriverState({rl_pos, rl_vel, rl_current, {0, 0}, {0, 0}})));
-  ON_CALL(*robot_driver_->GetMockRRMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Return(
-      panther_hardware_interfaces::MotorDriverState({rr_pos, rr_vel, rr_current, {0, 0}, {0, 0}})));
-
-  robot_driver_->UpdateMotorsState();
-
-  const auto & fl_data = robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName)
-                           .GetMotorState(PantherMotorChannel::LEFT);
-  const auto & fr_data = robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName)
-                           .GetMotorState(PantherMotorChannel::RIGHT);
-  const auto & rl_data = robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName)
-                           .GetMotorState(PantherMotorChannel::LEFT);
-  const auto & rr_data = robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName)
-                           .GetMotorState(PantherMotorChannel::RIGHT);
-
-  EXPECT_FLOAT_EQ(fl_data.GetPosition(), fl_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(fl_data.GetVelocity(), fl_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(fl_data.GetTorque(), fl_current * kRbtqCurrentFbToNewtonMeters);
-
-  EXPECT_FLOAT_EQ(fr_data.GetPosition(), fr_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(fr_data.GetVelocity(), fr_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(fr_data.GetTorque(), fr_current * kRbtqCurrentFbToNewtonMeters);
-
-  EXPECT_FLOAT_EQ(rl_data.GetPosition(), rl_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(rl_data.GetVelocity(), rl_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(rl_data.GetTorque(), rl_current * kRbtqCurrentFbToNewtonMeters);
-
-  EXPECT_FLOAT_EQ(rr_data.GetPosition(), rr_pos * kRbtqPosFbToRad);
-  EXPECT_FLOAT_EQ(rr_data.GetVelocity(), rr_vel * kRbtqVelFbToRadPerSec);
-  EXPECT_FLOAT_EQ(rr_data.GetTorque(), rr_current * kRbtqCurrentFbToNewtonMeters);
-}
-
-TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimestamps)
-{
-  auto read_motor_driver_state_method = []() {
-    panther_hardware_interfaces::MotorDriverState state;
-    clock_gettime(CLOCK_MONOTONIC, &state.pos_timestamp);
-    clock_gettime(CLOCK_MONOTONIC, &state.vel_current_timestamp);
-    return state;
-  };
-
-  ON_CALL(*robot_driver_->GetMockFLMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
-  ON_CALL(*robot_driver_->GetMockFRMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
-  ON_CALL(*robot_driver_->GetMockRLMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
-  ON_CALL(*robot_driver_->GetMockRRMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
-
-  robot_driver_->UpdateMotorsState();
-
-  // sleep for timeout and check if timestamps were updated correctly
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
-    std::chrono::milliseconds(10));
-
-  robot_driver_->UpdateMotorsState();
-
-  EXPECT_FALSE(robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName)
-                 .IsMotorStatesDataTimedOut());
-  EXPECT_FALSE(
-    robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsMotorStatesDataTimedOut());
-}
-
-TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimeout)
-{
-  timespec current_time;
-  clock_gettime(CLOCK_MONOTONIC, &current_time);
-
-  panther_hardware_interfaces::MotorDriverState state = {0, 0, 0, current_time, current_time};
-
-  ON_CALL(*robot_driver_->GetMockFLMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Return(state));
-  ON_CALL(*robot_driver_->GetMockFRMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Return(state));
-  ON_CALL(*robot_driver_->GetMockRLMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Return(state));
-  ON_CALL(*robot_driver_->GetMockRRMotorDriver(), ReadMotorDriverState())
-    .WillByDefault(::testing::Return(state));
-
-  // sleep for pdo_motor_states_timeout_ms + 10ms
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
-    std::chrono::milliseconds(10));
-
-  robot_driver_->UpdateMotorsState();
-
-  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName)
-                .IsMotorStatesDataTimedOut());
-  EXPECT_TRUE(
-    robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsMotorStatesDataTimedOut());
-  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName).IsError());
-  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsError());
-}
-
-TEST_F(TestPantherRobotDriver, UpdateDriverState)
-{
-  using panther_hardware_interfaces::PantherMotorChannel;
-
-  const std::int16_t f_temp = 30;
-  const std::int16_t r_temp = 32;
-  const std::int16_t f_heatsink_temp = 31;
-  const std::int16_t r_heatsink_temp = 33;
-  const std::uint16_t f_volt = 400;
-  const std::uint16_t r_volt = 430;
-  const std::int16_t f_battery_current_1 = 10;
-  const std::int16_t r_battery_current_1 = 30;
-  const std::int16_t f_battery_current_2 = 30;
-  const std::int16_t r_battery_current_2 = 40;
-
-  const std::uint8_t fault_flag_overheat = static_cast<std::uint8_t>(0b01);
-  const std::uint8_t fault_flag_overvoltage = static_cast<std::uint8_t>(0b10);
-  const std::uint8_t script_flag_encoder_disconnected = static_cast<std::uint8_t>(0b10);
-  const std::uint8_t script_flag_amp_limiter = static_cast<std::uint8_t>(0b100);
-  const std::uint8_t runtime_error_loop_error = static_cast<std::uint8_t>(0b100);
-  const std::uint8_t runtime_error_safety_stop_active = static_cast<std::uint8_t>(0b1000);
-  const std::uint8_t runtime_error_forward_limit_triggered = static_cast<std::uint8_t>(0b10000);
-  const std::uint8_t runtime_error_reverse_limit_triggered = static_cast<std::uint8_t>(0b100000);
-
-  ON_CALL(*robot_driver_->GetMockFrontDriver(), ReadDriverState())
-    .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
-      {fault_flag_overheat,
-       script_flag_encoder_disconnected,
-       runtime_error_loop_error,
-       runtime_error_safety_stop_active,
-       f_battery_current_1,
-       f_battery_current_2,
-       f_volt,
-       f_temp,
-       f_heatsink_temp,
-       {0, 0},
-       {0, 0}})));
-  ON_CALL(*robot_driver_->GetMockRearDriver(), ReadDriverState())
-    .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
-      {fault_flag_overvoltage,
-       script_flag_amp_limiter,
-       runtime_error_forward_limit_triggered,
-       runtime_error_reverse_limit_triggered,
-       r_battery_current_1,
-       r_battery_current_2,
-       r_volt,
-       r_temp,
-       r_heatsink_temp,
-       {0, 0},
-       {0, 0}})));
-
-  robot_driver_->UpdateDriversState();
-
-  const auto & front_data = robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName);
-  const auto & rear_data = robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName);
-  const auto & front_driver_state =
-    robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName).GetDriverState();
-  const auto & rear_driver_state =
-    robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).GetDriverState();
-
-  EXPECT_EQ(static_cast<std::int16_t>(front_driver_state.GetTemperature()), f_temp);
-  EXPECT_EQ(
-    static_cast<std::int16_t>(front_driver_state.GetHeatsinkTemperature()), f_heatsink_temp);
-  EXPECT_EQ(static_cast<std::uint16_t>(front_driver_state.GetVoltage() * 10.0), f_volt);
-  EXPECT_EQ(
-    static_cast<std::int16_t>(front_driver_state.GetCurrent() * 10.0),
-    f_battery_current_1 + f_battery_current_2);
-
-  EXPECT_EQ(static_cast<std::int16_t>(rear_driver_state.GetTemperature()), r_temp);
-  EXPECT_EQ(static_cast<std::int16_t>(rear_driver_state.GetHeatsinkTemperature()), r_heatsink_temp);
-  EXPECT_EQ(static_cast<std::uint16_t>(rear_driver_state.GetVoltage() * 10.0), r_volt);
-  EXPECT_EQ(
-    static_cast<std::int16_t>(rear_driver_state.GetCurrent() * 10.0),
-    r_battery_current_1 + r_battery_current_2);
-
-  EXPECT_TRUE(front_data.GetFaultFlag().GetMessage().overheat);
-  EXPECT_TRUE(front_data.GetScriptFlag().GetMessage().encoder_disconnected);
-  EXPECT_TRUE(front_data.GetRuntimeError(PantherMotorChannel::RIGHT).GetMessage().loop_error);
-  EXPECT_TRUE(
-    front_data.GetRuntimeError(PantherMotorChannel::LEFT).GetMessage().safety_stop_active);
-
-  EXPECT_TRUE(rear_data.GetFaultFlag().GetMessage().overvoltage);
-  EXPECT_TRUE(rear_data.GetScriptFlag().GetMessage().amp_limiter);
-  EXPECT_TRUE(
-    rear_data.GetRuntimeError(PantherMotorChannel::RIGHT).GetMessage().forward_limit_triggered);
-  EXPECT_TRUE(
-    rear_data.GetRuntimeError(PantherMotorChannel::LEFT).GetMessage().reverse_limit_triggered);
-}
-
-TEST_F(TestPantherRobotDriver, UpdateDriverStateTimestamps)
-{
-  auto read_driver_state_method = []() {
-    panther_hardware_interfaces::DriverState state;
-    clock_gettime(CLOCK_MONOTONIC, &state.flags_current_timestamp);
-    clock_gettime(CLOCK_MONOTONIC, &state.voltages_temps_timestamp);
-    return state;
-  };
-
-  ON_CALL(*robot_driver_->GetMockFrontDriver(), ReadDriverState())
-    .WillByDefault(::testing::Invoke(read_driver_state_method));
-  ON_CALL(*robot_driver_->GetMockRearDriver(), ReadDriverState())
-    .WillByDefault(::testing::Invoke(read_driver_state_method));
-
-  robot_driver_->UpdateDriversState();
-
-  // sleep for timeout and check if timestamps were updated correctly
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
-    std::chrono::milliseconds(10));
-
-  robot_driver_->UpdateDriversState();
-
-  EXPECT_FALSE(robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName)
-                 .IsDriverStateDataTimedOut());
-  EXPECT_FALSE(
-    robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsDriverStateDataTimedOut());
-}
-
-TEST_F(TestPantherRobotDriver, UpdateDriverStateTimeout)
-{
-  timespec current_time;
-  clock_gettime(CLOCK_MONOTONIC, &current_time);
-
-  panther_hardware_interfaces::DriverState state = {
-    0, 0, 0, 0, 0, 0, 0, 0, 0, current_time, current_time};
-
-  ON_CALL(*robot_driver_->GetMockFrontDriver(), ReadDriverState())
-    .WillByDefault(::testing::Return(state));
-  ON_CALL(*robot_driver_->GetMockRearDriver(), ReadDriverState())
-    .WillByDefault(::testing::Return(state));
-
-  // sleep for pdo_driver_state_timeout_ms + 10ms
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
-    std::chrono::milliseconds(10));
-
-  robot_driver_->UpdateDriversState();
-
-  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName)
-                .IsDriverStateDataTimedOut());
-  EXPECT_TRUE(
-    robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsDriverStateDataTimedOut());
-  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kFrontDriverName).IsError());
-  EXPECT_TRUE(robot_driver_->GetData(PantherRobotDriverWrapper::kRearDriverName).IsError());
-}
+protected:
+  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
+  std::unique_ptr<PantherRobotDriverWrapper> robot_driver_;
+};
 
 TEST_F(TestPantherRobotDriver, SendSpeedCommands)
 {
@@ -498,6 +154,8 @@ TEST_F(TestPantherRobotDriver, SendSpeedCommands)
   const float rl_v = 0.3;
   const float rr_v = 0.4;
 
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsCANError()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsCANError()).Times(1);
   EXPECT_CALL(
     *robot_driver_->GetMockFLMotorDriver(),
     SendCmdVel(::testing::Eq(static_cast<std::int32_t>(fl_v * kRadPerSecToRbtqCmd))))
@@ -515,62 +173,40 @@ TEST_F(TestPantherRobotDriver, SendSpeedCommands)
     SendCmdVel(::testing::Eq(static_cast<std::int32_t>(rr_v * kRadPerSecToRbtqCmd))))
     .Times(1);
 
-  EXPECT_NO_THROW(robot_driver_->SendSpeedCommands(fl_v, fr_v, rl_v, rr_v));
-}
-
-TEST_F(TestPantherRobotDriver, TurnOnEStop)
-{
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOnEStop()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOnEStop()).Times(1);
-
-  EXPECT_NO_THROW(robot_driver_->TurnOnEStop());
-}
-
-TEST_F(TestPantherRobotDriver, TurnOffEStop)
-{
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOffEStop()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOffEStop()).Times(1);
-
-  EXPECT_NO_THROW(robot_driver_->TurnOffEStop());
+  const std::vector<float> speeds = {fl_v, fr_v, rl_v, rr_v};
+  EXPECT_NO_THROW(robot_driver_->SendSpeedCommands(speeds));
 }
 
-TEST_F(TestPantherRobotDriver, TurnOnEStopError)
+TEST_F(TestPantherRobotDriver, SendSpeedCommandsSendCmdVelError)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOnEStop())
+  EXPECT_CALL(*robot_driver_->GetMockFLMotorDriver(), SendCmdVel(::testing::_))
     .WillOnce(::testing::Throw(std::runtime_error("")));
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOnEStop()).Times(0);
 
-  EXPECT_THROW(robot_driver_->TurnOnEStop(), std::runtime_error);
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { robot_driver_->SendSpeedCommands({0.0, 0.0, 0.0, 0.0}); },
+    "Front driver send Roboteq cmd failed:"));
 }
 
-TEST_F(TestPantherRobotDriver, TurnOffEStopError)
+TEST_F(TestPantherRobotDriver, SendSpeedCommandsCANError)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOffEStop())
-    .WillOnce(::testing::Throw(std::runtime_error("")));
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOffEStop()).Times(0);
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsCANError()).WillOnce(::testing::Return(true));
 
-  EXPECT_THROW(robot_driver_->TurnOffEStop(), std::runtime_error);
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { robot_driver_->SendSpeedCommands({0.0, 0.0, 0.0, 0.0}); },
+    "CAN error detected on the front driver when trying to write speed commands."));
 }
 
-TEST_F(TestPantherRobotDriver, SafetyStop)
+TEST_F(TestPantherRobotDriver, SendSpeedCommandsInvalidVectorSize)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFLMotorDriver(), TurnOnSafetyStop()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockFRMotorDriver(), TurnOnSafetyStop()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRLMotorDriver(), TurnOnSafetyStop()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRRMotorDriver(), TurnOnSafetyStop()).Times(1);
+  const std::vector<float> speeds = {0.1, 0.2, 0.3};
 
-  EXPECT_NO_THROW(robot_driver_->TurnOnSafetyStop());
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { robot_driver_->SendSpeedCommands(speeds); }, "Invalid speeds vector size"));
 }
 
-TEST_F(TestPantherRobotDriver, SafetyStopError)
+TEST_F(TestPantherRobotDriver, AttemptErrorFlagResetWithZeroSpeed)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFLMotorDriver(), TurnOnSafetyStop())
-    .WillOnce(::testing::Throw(std::runtime_error("")));
-  EXPECT_CALL(*robot_driver_->GetMockFRMotorDriver(), TurnOnSafetyStop()).Times(0);
-  EXPECT_CALL(*robot_driver_->GetMockRLMotorDriver(), TurnOnSafetyStop()).Times(0);
-  EXPECT_CALL(*robot_driver_->GetMockRRMotorDriver(), TurnOnSafetyStop()).Times(0);
-
-  EXPECT_THROW(robot_driver_->TurnOnSafetyStop(), std::runtime_error);
+  EXPECT_NO_THROW(robot_driver_->AttemptErrorFlagResetWithZeroSpeed());
 }
 
 int main(int argc, char ** argv)
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
new file mode 100644
index 00000000..33ffb06b
--- /dev/null
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
@@ -0,0 +1,569 @@
+// Copyright 2023 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 <chrono>
+#include <cstdint>
+#include <future>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <thread>
+
+#include <gmock/gmock.h>
+
+#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
+
+#include "utils/fake_can_socket.hpp"
+#include "utils/mock_driver.hpp"
+#include "utils/test_constants.hpp"
+
+#include "panther_utils/test/test_utils.hpp"
+
+class RoboteqRobotDriverWrapper : public panther_hardware_interfaces::RoboteqRobotDriver
+{
+public:
+  RoboteqRobotDriverWrapper(
+    const panther_hardware_interfaces::CANopenSettings & canopen_settings,
+    const panther_hardware_interfaces::DrivetrainSettings & drivetrain_settings,
+    const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000))
+  : RoboteqRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time)
+  {
+    // Assume 2 drivers and 4 motor drivers
+    mock_fl_motor_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+    mock_fr_motor_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+    mock_rl_motor_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+    mock_rr_motor_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+
+    mock_front_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
+    mock_front_driver_->AddMotorDriver(kLeftMotorDriverName, mock_fl_motor_driver_);
+    mock_front_driver_->AddMotorDriver(kRightMotorDriverName, mock_fr_motor_driver_);
+
+    mock_rear_driver_ =
+      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
+    mock_rear_driver_->AddMotorDriver(kLeftMotorDriverName, mock_rl_motor_driver_);
+    mock_rear_driver_->AddMotorDriver(kRightMotorDriverName, mock_rr_motor_driver_);
+  }
+
+  void DefineDrivers() override
+  {
+    drivers_.emplace(kFrontDriverName, mock_front_driver_);
+    drivers_.emplace(kRearDriverName, mock_rear_driver_);
+  }
+
+  void SendSpeedCommands(const std::vector<float> & /*velocities*/) override {}
+  void AttemptErrorFlagResetWithZeroSpeed() override {}
+
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
+  GetMockFrontDriver()
+  {
+    return mock_front_driver_;
+  }
+
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
+  GetMockRearDriver()
+  {
+    return mock_rear_driver_;
+  }
+
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  GetMockFLMotorDriver()
+  {
+    return mock_fl_motor_driver_;
+  }
+
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  GetMockFRMotorDriver()
+  {
+    return mock_fr_motor_driver_;
+  }
+
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  GetMockRLMotorDriver()
+  {
+    return mock_rl_motor_driver_;
+  }
+
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  GetMockRRMotorDriver()
+  {
+    return mock_rr_motor_driver_;
+  }
+
+  static constexpr char kFrontDriverName[] = "front";
+  static constexpr char kRearDriverName[] = "rear";
+  static constexpr char kLeftMotorDriverName[] = "left";
+  static constexpr char kRightMotorDriverName[] = "right";
+
+private:
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
+    mock_front_driver_;
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
+    mock_rear_driver_;
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+    mock_fl_motor_driver_;
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+    mock_fr_motor_driver_;
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+    mock_rl_motor_driver_;
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+    mock_rr_motor_driver_;
+};
+
+class TestPantherRobotDriverInitialization : public ::testing::Test
+{
+public:
+  TestPantherRobotDriverInitialization()
+  {
+    can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
+      panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
+    can_socket_->Initialize();
+
+    robot_driver_ = std::make_unique<RoboteqRobotDriverWrapper>(
+      panther_hardware_interfaces_test::kCANopenSettings,
+      panther_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10));
+  }
+
+  ~TestPantherRobotDriverInitialization() {}
+
+protected:
+  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
+  std::unique_ptr<RoboteqRobotDriverWrapper> robot_driver_;
+};
+
+class TestPantherRobotDriver : public TestPantherRobotDriverInitialization
+{
+public:
+  TestPantherRobotDriver()
+  {
+    robot_driver_->Initialize();
+    robot_driver_->Activate();
+  }
+
+  ~TestPantherRobotDriver() { robot_driver_->Deinitialize(); }
+};
+
+TEST_F(TestPantherRobotDriverInitialization, Initialize)
+{
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), Boot()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), Boot()).Times(1);
+
+  EXPECT_NO_THROW(robot_driver_->Initialize());
+  ASSERT_NO_THROW(robot_driver_->Deinitialize());
+
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), Boot()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), Boot()).Times(1);
+  // Check if deinitialization worked correctly - initialize once again
+  EXPECT_NO_THROW(robot_driver_->Initialize());
+}
+
+TEST_F(TestPantherRobotDriverInitialization, Activate)
+{
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), ResetScript()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), ResetScript()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFLMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFRMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRLMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRRMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->Initialize());
+  ASSERT_NO_THROW(robot_driver_->Activate());
+}
+
+TEST_F(TestPantherRobotDriver, GetData)
+{
+  EXPECT_NO_THROW(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName));
+  EXPECT_NO_THROW(robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName));
+}
+
+TEST_F(TestPantherRobotDriver, GetDataError)
+{
+  const std::string name = "invalid_name";
+  const std::string error_msg = "Data with name '" + name + "' does not exist.";
+
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&] { robot_driver_->GetData(name); }, error_msg));
+}
+
+TEST_F(TestPantherRobotDriver, UpdateCommunicationState)
+{
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsCANError()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsHeartbeatTimeout()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsCANError()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsHeartbeatTimeout()).Times(1);
+
+  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
+}
+
+TEST_F(TestPantherRobotDriver, UpdateCommunicationStateCANErorr)
+{
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsCANError()).WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsCANError()).WillOnce(::testing::Return(true));
+
+  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
+
+  EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName).IsCANError());
+  EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsCANError());
+}
+
+TEST_F(TestPantherRobotDriver, UpdateCommunicationStateHeartbeatTimeout)
+{
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsHeartbeatTimeout())
+    .WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsHeartbeatTimeout())
+    .WillOnce(::testing::Return(true));
+
+  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
+
+  EXPECT_TRUE(
+    robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName).IsHeartbeatTimeout());
+  EXPECT_TRUE(
+    robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsHeartbeatTimeout());
+}
+
+TEST_F(TestPantherRobotDriver, UpdateMotorsState)
+{
+  using panther_hardware_interfaces::MotorChannels;
+  using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  using panther_hardware_interfaces_test::kRbtqPosFbToRad;
+  using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+
+  const std::int32_t fl_pos = 101;
+  const std::int32_t fl_vel = 102;
+  const std::int32_t fl_current = 103;
+  const std::int32_t fr_pos = 201;
+  const std::int32_t fr_vel = 202;
+  const std::int32_t fr_current = 203;
+  const std::int32_t rl_pos = 301;
+  const std::int32_t rl_vel = 302;
+  const std::int32_t rl_current = 303;
+  const std::int32_t rr_pos = 401;
+  const std::int32_t rr_vel = 402;
+  const std::int32_t rr_current = 403;
+
+  ON_CALL(*robot_driver_->GetMockFLMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Return(
+      panther_hardware_interfaces::MotorDriverState({fl_pos, fl_vel, fl_current, {0, 0}, {0, 0}})));
+  ON_CALL(*robot_driver_->GetMockFRMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Return(
+      panther_hardware_interfaces::MotorDriverState({fr_pos, fr_vel, fr_current, {0, 0}, {0, 0}})));
+  ON_CALL(*robot_driver_->GetMockRLMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Return(
+      panther_hardware_interfaces::MotorDriverState({rl_pos, rl_vel, rl_current, {0, 0}, {0, 0}})));
+  ON_CALL(*robot_driver_->GetMockRRMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Return(
+      panther_hardware_interfaces::MotorDriverState({rr_pos, rr_vel, rr_current, {0, 0}, {0, 0}})));
+
+  robot_driver_->UpdateMotorsState();
+
+  const auto & fl_data = robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName)
+                           .GetMotorState(MotorChannels::LEFT);
+  const auto & fr_data = robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName)
+                           .GetMotorState(MotorChannels::RIGHT);
+  const auto & rl_data = robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName)
+                           .GetMotorState(MotorChannels::LEFT);
+  const auto & rr_data = robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName)
+                           .GetMotorState(MotorChannels::RIGHT);
+
+  EXPECT_FLOAT_EQ(fl_data.GetPosition(), fl_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(fl_data.GetVelocity(), fl_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(fl_data.GetTorque(), fl_current * kRbtqCurrentFbToNewtonMeters);
+
+  EXPECT_FLOAT_EQ(fr_data.GetPosition(), fr_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(fr_data.GetVelocity(), fr_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(fr_data.GetTorque(), fr_current * kRbtqCurrentFbToNewtonMeters);
+
+  EXPECT_FLOAT_EQ(rl_data.GetPosition(), rl_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(rl_data.GetVelocity(), rl_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(rl_data.GetTorque(), rl_current * kRbtqCurrentFbToNewtonMeters);
+
+  EXPECT_FLOAT_EQ(rr_data.GetPosition(), rr_pos * kRbtqPosFbToRad);
+  EXPECT_FLOAT_EQ(rr_data.GetVelocity(), rr_vel * kRbtqVelFbToRadPerSec);
+  EXPECT_FLOAT_EQ(rr_data.GetTorque(), rr_current * kRbtqCurrentFbToNewtonMeters);
+}
+
+TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimestamps)
+{
+  auto read_motor_driver_state_method = []() {
+    panther_hardware_interfaces::MotorDriverState state;
+    clock_gettime(CLOCK_MONOTONIC, &state.pos_timestamp);
+    clock_gettime(CLOCK_MONOTONIC, &state.vel_current_timestamp);
+    return state;
+  };
+
+  ON_CALL(*robot_driver_->GetMockFLMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
+  ON_CALL(*robot_driver_->GetMockFRMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
+  ON_CALL(*robot_driver_->GetMockRLMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
+  ON_CALL(*robot_driver_->GetMockRRMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
+
+  robot_driver_->UpdateMotorsState();
+
+  // sleep for timeout and check if timestamps were updated correctly
+  std::this_thread::sleep_for(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
+    std::chrono::milliseconds(10));
+
+  robot_driver_->UpdateMotorsState();
+
+  EXPECT_FALSE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName)
+                 .IsMotorStatesDataTimedOut());
+  EXPECT_FALSE(
+    robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsMotorStatesDataTimedOut());
+}
+
+TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimeout)
+{
+  timespec current_time;
+  clock_gettime(CLOCK_MONOTONIC, &current_time);
+
+  panther_hardware_interfaces::MotorDriverState state = {0, 0, 0, current_time, current_time};
+
+  ON_CALL(*robot_driver_->GetMockFLMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Return(state));
+  ON_CALL(*robot_driver_->GetMockFRMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Return(state));
+  ON_CALL(*robot_driver_->GetMockRLMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Return(state));
+  ON_CALL(*robot_driver_->GetMockRRMotorDriver(), ReadMotorDriverState())
+    .WillByDefault(::testing::Return(state));
+
+  // sleep for pdo_motor_states_timeout_ms + 10ms
+  std::this_thread::sleep_for(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
+    std::chrono::milliseconds(10));
+
+  robot_driver_->UpdateMotorsState();
+
+  EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName)
+                .IsMotorStatesDataTimedOut());
+  EXPECT_TRUE(
+    robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsMotorStatesDataTimedOut());
+  EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName).IsError());
+  EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsError());
+}
+
+TEST_F(TestPantherRobotDriver, UpdateDriverState)
+{
+  using panther_hardware_interfaces::MotorChannels;
+
+  const std::int16_t f_temp = 30;
+  const std::int16_t r_temp = 32;
+  const std::int16_t f_heatsink_temp = 31;
+  const std::int16_t r_heatsink_temp = 33;
+  const std::uint16_t f_volt = 400;
+  const std::uint16_t r_volt = 430;
+  const std::int16_t f_battery_current_1 = 10;
+  const std::int16_t r_battery_current_1 = 30;
+  const std::int16_t f_battery_current_2 = 30;
+  const std::int16_t r_battery_current_2 = 40;
+
+  const std::uint8_t fault_flag_overheat = static_cast<std::uint8_t>(0b01);
+  const std::uint8_t fault_flag_overvoltage = static_cast<std::uint8_t>(0b10);
+  const std::uint8_t script_flag_encoder_disconnected = static_cast<std::uint8_t>(0b10);
+  const std::uint8_t script_flag_amp_limiter = static_cast<std::uint8_t>(0b100);
+  const std::uint8_t runtime_error_loop_error = static_cast<std::uint8_t>(0b100);
+  const std::uint8_t runtime_error_safety_stop_active = static_cast<std::uint8_t>(0b1000);
+  const std::uint8_t runtime_error_forward_limit_triggered = static_cast<std::uint8_t>(0b10000);
+  const std::uint8_t runtime_error_reverse_limit_triggered = static_cast<std::uint8_t>(0b100000);
+
+  ON_CALL(*robot_driver_->GetMockFrontDriver(), ReadDriverState())
+    .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
+      {fault_flag_overheat,
+       script_flag_encoder_disconnected,
+       runtime_error_loop_error,
+       runtime_error_safety_stop_active,
+       f_battery_current_1,
+       f_battery_current_2,
+       f_volt,
+       f_temp,
+       f_heatsink_temp,
+       {0, 0},
+       {0, 0}})));
+  ON_CALL(*robot_driver_->GetMockRearDriver(), ReadDriverState())
+    .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
+      {fault_flag_overvoltage,
+       script_flag_amp_limiter,
+       runtime_error_forward_limit_triggered,
+       runtime_error_reverse_limit_triggered,
+       r_battery_current_1,
+       r_battery_current_2,
+       r_volt,
+       r_temp,
+       r_heatsink_temp,
+       {0, 0},
+       {0, 0}})));
+
+  robot_driver_->UpdateDriversState();
+
+  const auto & front_data = robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName);
+  const auto & rear_data = robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName);
+  const auto & front_driver_state =
+    robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName).GetDriverState();
+  const auto & rear_driver_state =
+    robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).GetDriverState();
+
+  EXPECT_EQ(static_cast<std::int16_t>(front_driver_state.GetTemperature()), f_temp);
+  EXPECT_EQ(
+    static_cast<std::int16_t>(front_driver_state.GetHeatsinkTemperature()), f_heatsink_temp);
+  EXPECT_EQ(static_cast<std::uint16_t>(front_driver_state.GetVoltage() * 10.0), f_volt);
+  EXPECT_EQ(
+    static_cast<std::int16_t>(front_driver_state.GetCurrent() * 10.0),
+    f_battery_current_1 + f_battery_current_2);
+
+  EXPECT_EQ(static_cast<std::int16_t>(rear_driver_state.GetTemperature()), r_temp);
+  EXPECT_EQ(static_cast<std::int16_t>(rear_driver_state.GetHeatsinkTemperature()), r_heatsink_temp);
+  EXPECT_EQ(static_cast<std::uint16_t>(rear_driver_state.GetVoltage() * 10.0), r_volt);
+  EXPECT_EQ(
+    static_cast<std::int16_t>(rear_driver_state.GetCurrent() * 10.0),
+    r_battery_current_1 + r_battery_current_2);
+
+  EXPECT_TRUE(front_data.GetFaultFlag().GetMessage().overheat);
+  EXPECT_TRUE(front_data.GetScriptFlag().GetMessage().encoder_disconnected);
+  EXPECT_TRUE(front_data.GetRuntimeError(MotorChannels::RIGHT).GetMessage().loop_error);
+  EXPECT_TRUE(front_data.GetRuntimeError(MotorChannels::LEFT).GetMessage().safety_stop_active);
+
+  EXPECT_TRUE(rear_data.GetFaultFlag().GetMessage().overvoltage);
+  EXPECT_TRUE(rear_data.GetScriptFlag().GetMessage().amp_limiter);
+  EXPECT_TRUE(rear_data.GetRuntimeError(MotorChannels::RIGHT).GetMessage().forward_limit_triggered);
+  EXPECT_TRUE(rear_data.GetRuntimeError(MotorChannels::LEFT).GetMessage().reverse_limit_triggered);
+}
+
+TEST_F(TestPantherRobotDriver, UpdateDriverStateTimestamps)
+{
+  auto read_driver_state_method = []() {
+    panther_hardware_interfaces::DriverState state;
+    clock_gettime(CLOCK_MONOTONIC, &state.flags_current_timestamp);
+    clock_gettime(CLOCK_MONOTONIC, &state.voltages_temps_timestamp);
+    return state;
+  };
+
+  ON_CALL(*robot_driver_->GetMockFrontDriver(), ReadDriverState())
+    .WillByDefault(::testing::Invoke(read_driver_state_method));
+  ON_CALL(*robot_driver_->GetMockRearDriver(), ReadDriverState())
+    .WillByDefault(::testing::Invoke(read_driver_state_method));
+
+  robot_driver_->UpdateDriversState();
+
+  // sleep for timeout and check if timestamps were updated correctly
+  std::this_thread::sleep_for(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
+    std::chrono::milliseconds(10));
+
+  robot_driver_->UpdateDriversState();
+
+  EXPECT_FALSE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName)
+                 .IsDriverStateDataTimedOut());
+  EXPECT_FALSE(
+    robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsDriverStateDataTimedOut());
+}
+
+TEST_F(TestPantherRobotDriver, UpdateDriverStateTimeout)
+{
+  timespec current_time;
+  clock_gettime(CLOCK_MONOTONIC, &current_time);
+
+  panther_hardware_interfaces::DriverState state = {
+    0, 0, 0, 0, 0, 0, 0, 0, 0, current_time, current_time};
+
+  ON_CALL(*robot_driver_->GetMockFrontDriver(), ReadDriverState())
+    .WillByDefault(::testing::Return(state));
+  ON_CALL(*robot_driver_->GetMockRearDriver(), ReadDriverState())
+    .WillByDefault(::testing::Return(state));
+
+  // sleep for pdo_driver_state_timeout_ms + 10ms
+  std::this_thread::sleep_for(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
+    std::chrono::milliseconds(10));
+
+  robot_driver_->UpdateDriversState();
+
+  EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName)
+                .IsDriverStateDataTimedOut());
+  EXPECT_TRUE(
+    robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsDriverStateDataTimedOut());
+  EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName).IsError());
+  EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsError());
+}
+
+TEST_F(TestPantherRobotDriver, TurnOnEStop)
+{
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOnEStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOnEStop()).Times(1);
+
+  EXPECT_NO_THROW(robot_driver_->TurnOnEStop());
+}
+
+TEST_F(TestPantherRobotDriver, TurnOffEStop)
+{
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOffEStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOffEStop()).Times(1);
+
+  EXPECT_NO_THROW(robot_driver_->TurnOffEStop());
+}
+
+TEST_F(TestPantherRobotDriver, TurnOnEStopError)
+{
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOnEStop())
+    .WillOnce(::testing::Throw(std::runtime_error("")));
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOnEStop()).Times(0);
+
+  EXPECT_THROW(robot_driver_->TurnOnEStop(), std::runtime_error);
+}
+
+TEST_F(TestPantherRobotDriver, TurnOffEStopError)
+{
+  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOffEStop())
+    .WillOnce(::testing::Throw(std::runtime_error("")));
+  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOffEStop()).Times(0);
+
+  EXPECT_THROW(robot_driver_->TurnOffEStop(), std::runtime_error);
+}
+
+TEST_F(TestPantherRobotDriver, SafetyStop)
+{
+  EXPECT_CALL(*robot_driver_->GetMockFLMotorDriver(), TurnOnSafetyStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockFRMotorDriver(), TurnOnSafetyStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRLMotorDriver(), TurnOnSafetyStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->GetMockRRMotorDriver(), TurnOnSafetyStop()).Times(1);
+
+  EXPECT_NO_THROW(robot_driver_->TurnOnSafetyStop());
+}
+
+TEST_F(TestPantherRobotDriver, SafetyStopError)
+{
+  EXPECT_CALL(*robot_driver_->GetMockFLMotorDriver(), TurnOnSafetyStop())
+    .WillOnce(::testing::Throw(std::runtime_error("")));
+  EXPECT_CALL(*robot_driver_->GetMockFRMotorDriver(), TurnOnSafetyStop()).Times(0);
+  EXPECT_CALL(*robot_driver_->GetMockRLMotorDriver(), TurnOnSafetyStop()).Times(0);
+  EXPECT_CALL(*robot_driver_->GetMockRRMotorDriver(), TurnOnSafetyStop()).Times(0);
+
+  EXPECT_THROW(robot_driver_->TurnOnSafetyStop(), std::runtime_error);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

From 6b0cfe1cd1e718094c651ac36a334841dc60d033 Mon Sep 17 00:00:00 2001
From: KmakD <kmakd197@gmail.com>
Date: Wed, 4 Sep 2024 11:19:12 +0000
Subject: [PATCH 038/100] data timeout method

---
 .../robot_driver/roboteq_robot_driver.hpp     |  3 ++
 .../robot_driver/roboteq_robot_driver.cpp     | 29 ++++++++++---------
 2 files changed, 19 insertions(+), 13 deletions(-)

diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
index 1f1cd062..a64ab37b 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
@@ -160,6 +160,9 @@ class RoboteqRobotDriver : public RobotDriver
     RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
     const timespec & current_time);
   void SetDriverState(RoboteqData & data, const DriverState & state, const timespec & current_time);
+  bool DataTimeout(
+    const timespec & current_time, const timespec & data_timestamp,
+    const std::chrono::milliseconds & timeout);
 
   bool initialized_ = false;
 
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
index f0eaca37..66e6e336 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
@@ -207,13 +207,11 @@ void RoboteqRobotDriver::SetMotorsStates(
   RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
   const timespec & current_time)
 {
-  // TODO figure out both motors timestamps
-  bool data_timed_out =
-    (lely::util::from_timespec(current_time) - lely::util::from_timespec(left_state.pos_timestamp) >
-     pdo_motor_states_timeout_ms_) ||
-    (lely::util::from_timespec(current_time) -
-       lely::util::from_timespec(left_state.vel_current_timestamp) >
-     pdo_motor_states_timeout_ms_);
+  const bool data_timed_out =
+    DataTimeout(current_time, left_state.pos_timestamp, pdo_motor_states_timeout_ms_) ||
+    DataTimeout(current_time, left_state.vel_current_timestamp, pdo_motor_states_timeout_ms_) ||
+    DataTimeout(current_time, right_state.pos_timestamp, pdo_motor_states_timeout_ms_) ||
+    DataTimeout(current_time, right_state.vel_current_timestamp, pdo_motor_states_timeout_ms_);
 
   // Channel 1 - right, Channel 2 - left
   data.SetMotorsStates(right_state, left_state, data_timed_out);
@@ -222,14 +220,19 @@ void RoboteqRobotDriver::SetMotorsStates(
 void RoboteqRobotDriver::SetDriverState(
   RoboteqData & data, const DriverState & state, const timespec & current_time)
 {
-  bool data_timed_out = (lely::util::from_timespec(current_time) -
-                           lely::util::from_timespec(state.flags_current_timestamp) >
-                         pdo_driver_state_timeout_ms_) ||
-                        (lely::util::from_timespec(current_time) -
-                           lely::util::from_timespec(state.voltages_temps_timestamp) >
-                         pdo_driver_state_timeout_ms_);
+  const bool data_timed_out =
+    DataTimeout(current_time, state.flags_current_timestamp, pdo_driver_state_timeout_ms_) ||
+    DataTimeout(current_time, state.voltages_temps_timestamp, pdo_driver_state_timeout_ms_);
 
   data.SetDriverState(state, data_timed_out);
 }
 
+bool RoboteqRobotDriver::DataTimeout(
+  const timespec & current_time, const timespec & data_timestamp,
+  const std::chrono::milliseconds & timeout)
+{
+  return lely::util::from_timespec(current_time) - lely::util::from_timespec(data_timestamp) >
+         timeout;
+}
+
 }  // namespace panther_hardware_interfaces

From 973ea3635288a706e39536a074c66cfc222e2c62 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Thu, 5 Sep 2024 11:46:15 +0200
Subject: [PATCH 039/100] Save work

---
 panther_bringup/CMakeLists.txt                |   2 +-
 .../launch/controller.launch.py               |   2 +-
 .../config/configuration.yaml                 |   2 +-
 .../launch/simulate_multiple_robots.launch.py |  62 +++++------
 .../launch/simulate_robot.launch.py           |  26 ++---
 panther_gazebo/launch/simulation.launch.py    |  10 +-
 panther_gazebo/launch/spawn_robot.launch.py   |  60 ++--------
 panther_utils/panther_utils/arguments.py      | 103 +++++++++++-------
 8 files changed, 127 insertions(+), 140 deletions(-)
 rename {panther_bringup => panther_gazebo}/config/configuration.yaml (92%)

diff --git a/panther_bringup/CMakeLists.txt b/panther_bringup/CMakeLists.txt
index 121cdbd4..79305750 100644
--- a/panther_bringup/CMakeLists.txt
+++ b/panther_bringup/CMakeLists.txt
@@ -3,6 +3,6 @@ project(panther_bringup)
 
 find_package(ament_cmake REQUIRED)
 
-install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 
 ament_package()
diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py
index 256c4ee3..14e3f7c1 100644
--- a/panther_controller/launch/controller.launch.py
+++ b/panther_controller/launch/controller.launch.py
@@ -129,7 +129,7 @@ def generate_launch_description():
             "the 'wheel_config_path' and 'controller_config_path' arguments will be "
             "automatically adjusted and can be omitted."
         ),
-        choices=["WH01", "WH02", "WH04", "custom"],
+        choices=["WH01", "WH02", "WH04", "WH05", "custom"],
     )
 
     # Get URDF via xacro
diff --git a/panther_bringup/config/configuration.yaml b/panther_gazebo/config/configuration.yaml
similarity index 92%
rename from panther_bringup/config/configuration.yaml
rename to panther_gazebo/config/configuration.yaml
index 8db2ac51..c4ed5b5b 100644
--- a/panther_bringup/config/configuration.yaml
+++ b/panther_gazebo/config/configuration.yaml
@@ -10,7 +10,7 @@
 
 panther:
   robot_model: panther
-  init_pose: [0.0, 0.0, 0.0]
+  init_pose: [0.0, -2.0, 0.0]
   init_rotation: [0.0, 0.0, 0.0]
   configuration:
     wheel_type: WH01
diff --git a/panther_gazebo/launch/simulate_multiple_robots.launch.py b/panther_gazebo/launch/simulate_multiple_robots.launch.py
index 9a9dc497..6c8c1bc7 100644
--- a/panther_gazebo/launch/simulate_multiple_robots.launch.py
+++ b/panther_gazebo/launch/simulate_multiple_robots.launch.py
@@ -25,7 +25,7 @@
 )
 from launch_ros.actions import Node, SetUseSimTime
 from launch_ros.substitutions import FindPackageShare
-from nav2_common.launch import ParseMultiRobotPose
+from panther_utils.arguments import load_yaml_file, normalize_robot_configuration
 
 
 def generate_launch_description():
@@ -40,36 +40,32 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
-    declare_robots_arg = DeclareLaunchArgument(
-        "robots",
-        default_value=[],
-        description=(
-            "The list of the robots spawned in the simulation e. g. "
-            "robots:='robot1={x: 1.0, y: -2.0}; robot2={x: 1.0, y: -4.0}'."
-        ),
+    path = PathJoinSubstitution(
+        [FindPackageShare("panther_gazebo"), "config", "configuration.yaml"]
     )
-
-    robots_list = ParseMultiRobotPose("robots").value()
-    # If robots arg is empty, use default arguments from simulate_robot.launch.py
-    if len(robots_list) == 0:
-        robots_list = {
-            LaunchConfiguration("namespace", default=""): {
-                "x": LaunchConfiguration("x", default="0.0"),
-                "y": LaunchConfiguration("y", default="-2.0"),
-                "z": LaunchConfiguration("z", default="0.2"),
-                "roll": LaunchConfiguration("roll", default="0.0"),
-                "pitch": LaunchConfiguration("pitch", default="0.0"),
-                "yaw": LaunchConfiguration("yaw", default="0.0"),
-            }
-        }
-    else:
-        for robot_name, init_pose in robots_list.items():
-            robots_list[robot_name] = {k: str(v) for k, v in init_pose.items()}
+    yaml_data = load_yaml_file(path)
+    yaml_data = normalize_robot_configuration(yaml_data)
 
     simulate_robot = []
-    for idx, robot_name in enumerate(robots_list):
-        init_pose = robots_list[robot_name]
-        x, y, z, roll, pitch, yaw = [value for value in init_pose.values()]
+    idx = 0
+    for namespace, robot_config in yaml_data.items():
+        robot_model = robot_config.get("robot_model", "panther")
+        x, y, z = robot_config.get("init_pose", [0.0, 0.0, 0.0])
+        roll, pitch, yaw = robot_config.get("init_rotation", [0.0, 0.0, 0.0])
+        x, y, z, roll, pitch, yaw = map(str, [x, y, z, roll, pitch, yaw])
+        configuration_data = robot_config.get("configuration", {})
+        wheel_type = configuration_data.get("wheel_type", "")
+
+        # Prefer declaration over configuration:
+        namespace = LaunchConfiguration("namespace", default=namespace)
+        robot_model = LaunchConfiguration("robot_model", default=robot_model)
+        wheel_type = LaunchConfiguration("wheel_type", default=wheel_type)
+        x = LaunchConfiguration("x", default=x)
+        y = LaunchConfiguration("y", default=y)
+        z = LaunchConfiguration("z", default=z)
+        roll = LaunchConfiguration("roll", default=roll)
+        pitch = LaunchConfiguration("pitch", default=pitch)
+        yaw = LaunchConfiguration("yaw", default=yaw)
 
         spawn_robot_launch = IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
@@ -78,7 +74,9 @@ def generate_launch_description():
                 )
             ),
             launch_arguments={
-                "namespace": robot_name,
+                "namespace": namespace,
+                "robot_model": robot_model,
+                "wheel_type": wheel_type,
                 "x": x,
                 "y": y,
                 "z": z,
@@ -89,7 +87,7 @@ def generate_launch_description():
         )
 
         child_tf = PythonExpression(
-            ["'", robot_name, "' + '/odom' if '", robot_name, "' else 'odom'"]
+            ["'", namespace, "' + '/odom' if '", namespace, "' else 'odom'"]
         )
 
         world_transform = Node(
@@ -97,7 +95,7 @@ def generate_launch_description():
             executable="static_transform_publisher",
             name="static_tf_publisher",
             arguments=[x, y, z, roll, pitch, yaw, "world", child_tf],
-            namespace=robot_name,
+            namespace=namespace,
             emulate_tty=True,
             condition=IfCondition(add_world_transform),
         )
@@ -110,12 +108,12 @@ def generate_launch_description():
                 world_transform,
             ],
         )
+        idx += 1
         simulate_robot.append(group)
 
     return LaunchDescription(
         [
             declare_add_world_transform_arg,
-            declare_robots_arg,
             SetUseSimTime(True),
             *simulate_robot,
         ]
diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py
index 90435166..573df926 100644
--- a/panther_gazebo/launch/simulate_robot.launch.py
+++ b/panther_gazebo/launch/simulate_robot.launch.py
@@ -19,7 +19,6 @@
 from launch.conditions import IfCondition, UnlessCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
-    EnvironmentVariable,
     LaunchConfiguration,
     PathJoinSubstitution,
     PythonExpression,
@@ -27,6 +26,7 @@
 from launch_ros.actions import Node, SetUseSimTime
 from launch_ros.substitutions import FindPackageShare
 from nav2_common.launch import ReplaceString
+from panther_utils.arguments import declare_robot_args
 
 
 def generate_launch_description():
@@ -66,19 +66,7 @@ def generate_launch_description():
     )
 
     namespace = LaunchConfiguration("namespace")
-    declare_namespace_arg = DeclareLaunchArgument(
-        "namespace",
-        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
-        description="Add namespace to all launched nodes.",
-    )
-
-    robot = LaunchConfiguration("robot")
-    declare_robot_arg = DeclareLaunchArgument(
-        "robot",
-        default_value=EnvironmentVariable("ROBOT_MODEL", default_value="panther"),
-        description="Specify robot model.",
-        choices=["panther", "lynx"],
-    )
+    robot_model = LaunchConfiguration("robot_model")
 
     use_ekf = LaunchConfiguration("use_ekf")
     declare_use_ekf_arg = DeclareLaunchArgument(
@@ -97,7 +85,7 @@ def generate_launch_description():
         launch_arguments={
             "add_wheel_joints": "False",
             "namespace": namespace,
-            "robot": robot,
+            "robot_model": robot_model,
             "use_sim": "True",
         }.items(),
     )
@@ -194,13 +182,17 @@ def generate_launch_description():
         emulate_tty=True,
     )
 
+    path = PathJoinSubstitution(
+        [FindPackageShare("panther_gazebo"), "config", "configuration.yaml"]
+    )
+    list_of_robot_args = declare_robot_args(path)
+
     return LaunchDescription(
         [
+            *list_of_robot_args,
             declare_battery_config_path_arg,
             declare_components_config_path_arg,
             declare_gz_bridge_config_path_arg,
-            declare_namespace_arg,
-            declare_robot_arg,
             declare_use_ekf_arg,
             SetUseSimTime(True),
             spawn_robot_launch,
diff --git a/panther_gazebo/launch/simulation.launch.py b/panther_gazebo/launch/simulation.launch.py
index 04c192d7..2093b881 100644
--- a/panther_gazebo/launch/simulation.launch.py
+++ b/panther_gazebo/launch/simulation.launch.py
@@ -25,6 +25,7 @@
 from launch_ros.actions import SetUseSimTime
 from launch_ros.substitutions import FindPackageShare
 from nav2_common.launch import ReplaceString
+from panther_utils.arguments import load_yaml_file, normalize_robot_configuration
 
 
 def generate_launch_description():
@@ -38,10 +39,17 @@ def generate_launch_description():
         description="Run simulation with specific GUI layout.",
     )
 
+    path = PathJoinSubstitution(
+        [FindPackageShare("panther_gazebo"), "config", "configuration.yaml"]
+    )
+    yaml_data = load_yaml_file(path)
+    yaml_data = normalize_robot_configuration(yaml_data)
+    ns_from_config = list(yaml_data.keys())[0]
+
     namespace = LaunchConfiguration("namespace")
     declare_namespace_arg = DeclareLaunchArgument(
         "namespace",
-        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=ns_from_config),
         description="Add namespace to all launched nodes.",
     )
 
diff --git a/panther_gazebo/launch/spawn_robot.launch.py b/panther_gazebo/launch/spawn_robot.launch.py
index 0d43ba57..d7bcc378 100644
--- a/panther_gazebo/launch/spawn_robot.launch.py
+++ b/panther_gazebo/launch/spawn_robot.launch.py
@@ -15,65 +15,29 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.actions import IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
-    EnvironmentVariable,
     LaunchConfiguration,
     PathJoinSubstitution,
     PythonExpression,
 )
 from launch_ros.actions import Node, SetUseSimTime
 from launch_ros.substitutions import FindPackageShare
+from panther_utils.arguments import declare_robot_args
 from panther_utils.messages import welcome_msg
 
 
 def generate_launch_description():
 
     namespace = LaunchConfiguration("namespace")
-    declare_namespace_arg = DeclareLaunchArgument(
-        "namespace",
-        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
-        description="Add namespace to all launched nodes.",
-    )
-
-    robot = LaunchConfiguration("robot")
-    declare_robot_arg = DeclareLaunchArgument(
-        "robot",
-        default_value=EnvironmentVariable("ROBOT_MODEL", default_value="panther"),
-        description="Specify robot model.",
-        choices=["panther", "lynx"],
-    )
-
+    robot_model = LaunchConfiguration("robot_model")
     x = LaunchConfiguration("x")
-    declare_x_arg = DeclareLaunchArgument(
-        "x", default_value="0.0", description="Initial robot position in the global 'x' axis."
-    )
-
     y = LaunchConfiguration("y")
-    declare_y_arg = DeclareLaunchArgument(
-        "y", default_value="-2.0", description="Initial robot position in the global 'y' axis."
-    )
-
     z = LaunchConfiguration("z")
-    declare_z_arg = DeclareLaunchArgument(
-        "z", default_value="0.2", description="Initial robot position in the global 'z' axis."
-    )
-
     roll = LaunchConfiguration("roll")
-    declare_roll_arg = DeclareLaunchArgument(
-        "roll", default_value="0.0", description="Initial robot 'roll' orientation."
-    )
-
     pitch = LaunchConfiguration("pitch")
-    declare_pitch_arg = DeclareLaunchArgument(
-        "pitch", default_value="0.0", description="Initial robot 'pitch' orientation."
-    )
-
     yaw = LaunchConfiguration("yaw")
-    declare_yaw_arg = DeclareLaunchArgument(
-        "yaw", default_value="0.0", description="Initial robot 'yaw' orientation."
-    )
 
     log_stats = {
         "Robot namespace": namespace,
@@ -81,7 +45,7 @@ def generate_launch_description():
     }
     welcome_info = welcome_msg("---", "simulation", log_stats)
 
-    urdf_packages = PythonExpression(["'", robot, "_description'"])
+    urdf_packages = PythonExpression(["'", robot_model, "_description'"])
     add_wheel_joints = LaunchConfiguration("add_wheel_joints", default="True")
 
     load_urdf = IncludeLaunchDescription(
@@ -93,7 +57,7 @@ def generate_launch_description():
         launch_arguments={
             "add_wheel_joints": add_wheel_joints,
             "namespace": namespace,
-            "robot": robot,
+            "robot_model": robot_model,
             "use_sim": "True",
         }.items(),
     )
@@ -123,16 +87,14 @@ def generate_launch_description():
         emulate_tty=True,
     )
 
+    path = PathJoinSubstitution(
+        [FindPackageShare("panther_gazebo"), "config", "configuration.yaml"]
+    )
+    list_of_robot_args = declare_robot_args(path)
+
     return LaunchDescription(
         [
-            declare_namespace_arg,
-            declare_robot_arg,
-            declare_x_arg,
-            declare_y_arg,
-            declare_z_arg,
-            declare_roll_arg,
-            declare_pitch_arg,
-            declare_yaw_arg,
+            *list_of_robot_args,
             SetUseSimTime(True),
             welcome_info,
             load_urdf,
diff --git a/panther_utils/panther_utils/arguments.py b/panther_utils/panther_utils/arguments.py
index f7d3efb1..87443767 100644
--- a/panther_utils/panther_utils/arguments.py
+++ b/panther_utils/panther_utils/arguments.py
@@ -15,19 +15,17 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import sys
-from typing import Iterable, Tuple
+from typing import Tuple
 
 import yaml
 from launch import LaunchContext, Substitution
 from launch.actions import DeclareLaunchArgument
-from launch.substitutions import EnvironmentVariable, PathJoinSubstitution
-from launch_ros.substitutions import FindPackageShare
+from launch.substitutions import EnvironmentVariable, LaunchConfiguration
 
 # Define valid configurations for each robot model
 VALID_CONFIGURATIONS = {
-    "panther": {"wheel_type": ["WH01", "WH02", "WH03"]},
-    "lynx": {"wheel_type": ["WH04"]},
+    "panther": {"wheel_type": ["WH01", "WH02", "WH04"]},
+    "lynx": {"wheel_type": ["WH05"]},
 }
 
 
@@ -38,8 +36,9 @@ def resolve_path(path: str | Substitution) -> str:
     return path
 
 
-def load_yaml_file(path: str) -> dict:
+def load_yaml_file(path: str | Substitution) -> dict:
     """Load YAML file and return its contents."""
+    path = resolve_path(path)
     try:
         with open(path, "r") as file:
             return yaml.safe_load(file)
@@ -47,15 +46,7 @@ def load_yaml_file(path: str) -> dict:
         raise ValueError(f"Error reading YAML file: {exc}") from exc
 
 
-def load_robot_configuration(yaml_data: dict) -> Tuple[str, dict]:
-    """Retrieve first element data if exists; otherwise, return the base structure."""
-    if "configuration" not in yaml_data:
-        namespace = next(iter(yaml_data.keys()))
-        return namespace, yaml_data[namespace]
-    return "", yaml_data
-
-
-def validate_configuration(yaml_data: dict) -> None:
+def validate_robot_configuration(yaml_data: dict) -> None:
     """Validate the robot model and wheel type configuration."""
     robot_model = yaml_data.get("robot_model", "")
     configuration_data = yaml_data.get("configuration", {})
@@ -75,11 +66,58 @@ def validate_configuration(yaml_data: dict) -> None:
         )
 
 
-def create_launch_arguments(namespace: str, yaml_data: dict) -> Iterable[DeclareLaunchArgument]:
-    """Generate ROS 2 launch description based on the YAML configuration."""
-    x, y, z = yaml_data.get("initial_pose", [0.0, 0.0, 0.0])
-    roll, pitch, yaw = yaml_data.get("initial_rotation", [0.0, 0.0, 0.0])
-    configuration_data = yaml_data.get("configuration", {})
+def normalize_robot_configuration(yaml_data: dict) -> dict:
+    """Normalizes the YAML dictionary structure to a flat to nested format and validates the structure."""
+
+    obligatory_keys = {"configuration"}
+
+    # Checking and normalize flat structure
+    if obligatory_keys.issubset(yaml_data.keys()):
+        validate_robot_configuration(yaml_data)
+        return {"": yaml_data}
+
+    # Checking nested structure
+    if all(isinstance(value, dict) for value in yaml_data.values()):
+        nested_data = {}
+        for namespace, robot_config in yaml_data.items():
+            if not obligatory_keys.issubset(robot_config.keys()):
+                raise ValueError(
+                    f"Invalid nested YAML structure at '{namespace}': Missing required keys."
+                )
+            validate_robot_configuration(robot_config)
+            nested_data[namespace] = robot_config
+        return nested_data
+
+    raise ValueError("Invalid YAML structure: The data does not match expected formats.")
+
+
+def extract_single_robot_configuration(yaml_data: dict, idx: int = 0) -> Tuple[str, dict]:
+    """Extracts the namespace and configuration based on the provided index from a YAML dictionary."""
+    keys = list(yaml_data.keys())
+    namespace = keys[idx]
+    configuration = yaml_data[namespace]
+    return namespace, configuration
+
+
+def create_launch_arguments(namespace: str, robot_config: dict) -> list[DeclareLaunchArgument]:
+    """Declare launch arguments based on the YAML configuration files."""
+    robot_model = robot_config.get("robot_model", "panther")
+    x, y, z = robot_config.get("init_pose", [0.0, 0.0, 0.0])
+    roll, pitch, yaw = robot_config.get("init_rotation", [0.0, 0.0, 0.0])
+    x, y, z, roll, pitch, yaw = map(str, [x, y, z, roll, pitch, yaw])
+    configuration_data = robot_config.get("configuration", {})
+    wheel_type = configuration_data.get("wheel_type", "")
+
+    # Prefer declaration over configuration:
+    namespace = LaunchConfiguration("namespace", default=namespace)
+    robot_model = LaunchConfiguration("robot_model", default=robot_model)
+    wheel_type = LaunchConfiguration("wheel_type", default=wheel_type)
+    x = LaunchConfiguration("x", default=x)
+    y = LaunchConfiguration("y", default=y)
+    z = LaunchConfiguration("z", default=z)
+    roll = LaunchConfiguration("roll", default=roll)
+    pitch = LaunchConfiguration("pitch", default=pitch)
+    yaw = LaunchConfiguration("yaw", default=yaw)
 
     return [
         DeclareLaunchArgument(
@@ -89,14 +127,12 @@ def create_launch_arguments(namespace: str, yaml_data: dict) -> Iterable[Declare
         ),
         DeclareLaunchArgument(
             "robot_model",
-            default_value=EnvironmentVariable(
-                "ROBOT_MODEL", default_value=yaml_data.get("robot_model", "panther")
-            ),
+            default_value=EnvironmentVariable("ROBOT_MODEL", default_value=robot_model),
             description="Specify robot model type.",
         ),
         DeclareLaunchArgument(
             "wheel_type",
-            default_value=configuration_data["wheel_type"],
+            default_value=wheel_type,
             description=(
                 "Specify the wheel type. If the selected wheel type is not 'custom', "
                 "the 'wheel_config_path' and 'controller_config_path' arguments will be "
@@ -124,19 +160,10 @@ def create_launch_arguments(namespace: str, yaml_data: dict) -> Iterable[Declare
     ]
 
 
-def declare_robot_args(path: str | Substitution) -> Iterable[DeclareLaunchArgument]:
-    """Declare launch arguments based on the YAML configuration files."""
-    path = resolve_path(path)
+def declare_robot_args(path: str | Substitution) -> list[DeclareLaunchArgument]:
+    """Retrieves and validate the robot configuration from the YAML data."""
     yaml_data = load_yaml_file(path)
-    namespace, robot_config = load_robot_configuration(yaml_data)
-    try:
-        validate_configuration(robot_config)
-    except ValueError as error:
-        print(f"Validation Error: {error}")
-        sys.exit(1)
+    yaml_data = normalize_robot_configuration(yaml_data)
+    namespace, robot_config = extract_single_robot_configuration(yaml_data)
     list_of_args = create_launch_arguments(namespace, robot_config)
     return list_of_args
-
-
-path = PathJoinSubstitution([FindPackageShare("panther_bringup"), "config", "configuration.yaml"])
-print(declare_robot_args(path))

From 40e3c1701e8e5caecabf39821d24229c1113ef12 Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Mon, 9 Sep 2024 09:54:52 +0000
Subject: [PATCH 040/100] update briefs

---
 .../panther_system/robot_driver/driver.hpp            |  2 +-
 .../panther_system/robot_driver/lynx_robot_driver.hpp |  7 ++-----
 .../robot_driver/panther_robot_driver.hpp             |  8 +++-----
 .../panther_system/robot_driver/robot_driver.hpp      |  2 +-
 .../robot_driver/roboteq_robot_driver.hpp             | 11 ++++++-----
 5 files changed, 13 insertions(+), 17 deletions(-)

diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp
index f8544a5d..20193192 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp
@@ -87,7 +87,7 @@ class MotorDriver
 };
 
 /**
- * @brief Abstract class for  driver
+ * @brief Abstract class that provides functionality for managing motor drivers
  */
 class Driver
 {
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
index ce6547fd..9dd44ef4 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
@@ -27,11 +27,8 @@ namespace panther_hardware_interfaces
 {
 
 /**
- * @brief This class abstracts the usage of one Roboteq controller.
- * It uses canopen_manager for communication with Roboteq controller,
- * implements the activation procedure for controller (resets script and sends initial 0 command),
- * and provides methods to get data feedback and send commands.
- * Data is converted between raw Roboteq formats and SI units using roboteq_data_converters.
+ * @brief This class implements RoboteqRobotDriver for Lynx robot.
+ * It defines one Roboteq controller with two motors controlling left and right wheels.
  */
 class LynxRobotDriver : public RoboteqRobotDriver
 {
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
index ba401eae..f9f9d0f2 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
@@ -27,11 +27,9 @@ namespace panther_hardware_interfaces
 {
 
 /**
- * @brief This class abstracts the usage of two Roboteq controllers.
- * It uses canopen_manager for communication with Roboteq controllers,
- * implements the activation procedure for controllers (resets script and sends initial 0 command),
- * and provides methods to get data feedback and send commands.
- * Data is converted between raw Roboteq formats and SI units using roboteq_data_converters.
+ * @brief This class implements the RoboteqRobotDriver for the Panther robot.
+ * It defines two Roboteq drivers (front and rear) with two motors each for controlling the left
+ * and right wheels.
  */
 class PantherRobotDriver : public RoboteqRobotDriver
 {
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
index 02a2cead..87ec0210 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
@@ -24,7 +24,7 @@ namespace panther_hardware_interfaces
 {
 
 /**
- * @brief Abstract class for managing robot drivers.
+ * @brief Abstract class for managing different robot drivers.
  */
 class RobotDriver
 {
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
index a64ab37b..da2e9bca 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
@@ -47,11 +47,12 @@ struct MotorChannels
 };
 
 /**
- * @brief Abstract class for use with Roboteq controllers.
- * It uses canopen_manager for communication with Roboteq controllers,
- * implements the activation procedure for controllers (resets script and sends initial 0 command),
- * and provides methods to get data feedback and send commands.
- * Data is converted between raw Roboteq formats and SI units using roboteq_data_converters.
+ * @brief Abstract class implementing RobotDriver for robots using Roboteq drivers.
+ * It uses canopen_manager for communication with Roboteq controllers.
+ * This class implements the activation procedure for controllers, which involves resetting the
+ * script and sending an initial 0 command. It also provides methods to get data feedback and send
+ * commands. Data is converted between raw Roboteq formats and SI units using
+ * roboteq_data_converters.
  */
 class RoboteqRobotDriver : public RobotDriver
 {

From aece9215c8220f160b2dfe31fa57215a554957fc Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Tue, 10 Sep 2024 13:49:18 +0000
Subject: [PATCH 041/100] review fixes

---
 .../panther_system/robot_driver/driver.hpp    |  14 +-
 .../robot_driver/lynx_robot_driver.hpp        |   2 +-
 .../robot_driver/panther_robot_driver.hpp     |   5 +-
 .../robot_driver/robot_driver.hpp             |  16 +-
 .../robot_driver/roboteq_driver.hpp           |  17 +-
 .../robot_driver/roboteq_robot_driver.hpp     |  12 +-
 .../robot_driver/roboteq_driver.cpp           |   8 +-
 .../robot_driver/roboteq_robot_driver.cpp     |  19 +-
 .../robot_driver/test_lynx_robot_driver.cpp   |   4 +-
 .../test_panther_robot_driver.cpp             |   4 +-
 .../robot_driver/test_roboteq_driver.cpp      |  18 +-
 .../test_roboteq_robot_driver.cpp             | 298 ++++++++----------
 .../test/utils/mock_driver.hpp                |  15 +-
 13 files changed, 175 insertions(+), 257 deletions(-)

diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp
index 20193192..e4cc87c5 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp
@@ -60,14 +60,14 @@ struct DriverState
 /**
  * @brief Abstract class for motor driver
  */
-class MotorDriver
+class MotorDriverInterface
 {
 public:
   /**
    * @brief Reads motors' state data returned from  (PDO 1 and 2) and saves
    * last timestamps
    */
-  virtual MotorDriverState ReadMotorDriverState() = 0;
+  virtual MotorDriverState ReadState() = 0;
 
   /**
    * @brief Sends commands to the motors
@@ -89,7 +89,7 @@ class MotorDriver
 /**
  * @brief Abstract class that provides functionality for managing motor drivers
  */
-class Driver
+class DriverInterface
 {
 public:
   /**
@@ -114,7 +114,7 @@ class Driver
    * voltage, battery currents (for channel 1 and 2, they are not the same as motor currents),
    * temperatures. Also saves the last timestamps
    */
-  virtual DriverState ReadDriverState() = 0;
+  virtual DriverState ReadState() = 0;
 
   /**
    * @exception std::runtime_error if any operation returns error
@@ -135,14 +135,14 @@ class Driver
    * @brief Adds a motor driver to the driver
    */
   virtual void AddMotorDriver(
-    const std::string name, std::shared_ptr<MotorDriver> motor_driver) = 0;
+    const std::string name, std::shared_ptr<MotorDriverInterface> motor_driver) = 0;
 
-  virtual std::shared_ptr<MotorDriver> GetMotorDriver(const std::string & name) = 0;
+  virtual std::shared_ptr<MotorDriverInterface> GetMotorDriver(const std::string & name) = 0;
 
   /**
    * @brief Alias for a shared pointer to a Driver object.
    */
-  using SharedPtr = std::shared_ptr<Driver>;
+  using SharedPtr = std::shared_ptr<DriverInterface>;
 };
 
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
index 9dd44ef4..6449d7c6 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
@@ -56,7 +56,7 @@ class LynxRobotDriver : public RoboteqRobotDriver
    * @brief Attempt to clear driver error flags by sending 0 velocity commands to motors. If Roboteq
    * driver faults still exist, the error flag will remain active.
    */
-  inline void AttemptErrorFlagResetWithZeroSpeed() override { SendSpeedCommands({0.0, 0.0}); };
+  void AttemptErrorFlagReset() override { SendSpeedCommands({0.0, 0.0}); };
 
 protected:
   /**
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
index f9f9d0f2..40665e76 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
@@ -58,10 +58,7 @@ class PantherRobotDriver : public RoboteqRobotDriver
    * @brief Attempt to clear driver error flags by sending 0 velocity commands to motors. If Roboteq
    * driver faults still exist, the error flag will remain active.
    */
-  inline void AttemptErrorFlagResetWithZeroSpeed() override
-  {
-    SendSpeedCommands({0.0, 0.0, 0.0, 0.0});
-  };
+  void AttemptErrorFlagReset() override { SendSpeedCommands({0.0, 0.0, 0.0, 0.0}); };
 
 protected:
   /**
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
index 87ec0210..f7dce3c3 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
@@ -26,7 +26,7 @@ namespace panther_hardware_interfaces
 /**
  * @brief Abstract class for managing different robot drivers.
  */
-class RobotDriver
+class RobotDriverInterface
 {
 public:
   /**
@@ -103,18 +103,10 @@ class RobotDriver
   virtual void TurnOffEStop() = 0;
 
   /**
-   * @brief Turns on safety stop. To turn it off, it is necessary to send
-   * 0 commands to motors.
-   *
-   * @exception std::runtime_error if any operation returns error
-   */
-  virtual void TurnOnSafetyStop() = 0;
-
-  /**
-   * @brief Attempt to clear driver error flags by sending 0 velocity commands to motors. If driver
-   * faults still exist, the error flag will remain active.
+   * @brief Attempt to clear driver error flags. If driver faults still exist,
+   * the error flags should remain active.
    */
-  virtual inline void AttemptErrorFlagResetWithZeroSpeed() = 0;
+  virtual void AttemptErrorFlagReset() = 0;
 };
 
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp
index ee9815c2..a499120a 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp
@@ -37,7 +37,7 @@ class RoboteqMotorDriver;
  * @brief Hardware implementation of Driver with lely LoopDriver for Roboteq drivers
  * control
  */
-class RoboteqDriver : public Driver, public lely::canopen::LoopDriver
+class RoboteqDriver : public DriverInterface, public lely::canopen::LoopDriver
 {
 public:
   RoboteqDriver(
@@ -66,7 +66,7 @@ class RoboteqDriver : public Driver, public lely::canopen::LoopDriver
    * voltage, battery currents (for channel 1 and 2, they are not the same as motor currents),
    * temperatures. Also saves the last timestamps
    */
-  DriverState ReadDriverState() override;
+  DriverState ReadState() override;
 
   /**
    * @exception std::runtime_error if any operation returns error
@@ -86,14 +86,15 @@ class RoboteqDriver : public Driver, public lely::canopen::LoopDriver
   /**
    * @brief Adds a motor driver to the driver
    */
-  void AddMotorDriver(const std::string name, std::shared_ptr<MotorDriver> motor_driver) override;
+  void AddMotorDriver(
+    const std::string name, std::shared_ptr<MotorDriverInterface> motor_driver) override;
 
   /**
    * @brief Returns a motor driver by name
    *
    * @exception std::runtime_error if motor driver with the given name does not exist
    */
-  std::shared_ptr<MotorDriver> GetMotorDriver(const std::string & name) override;
+  std::shared_ptr<MotorDriverInterface> GetMotorDriver(const std::string & name) override;
 
   /**
    * @brief Blocking SDO write operation
@@ -156,13 +157,13 @@ class RoboteqDriver : public Driver, public lely::canopen::LoopDriver
 
   const std::chrono::milliseconds sdo_operation_timeout_ms_;
 
-  std::map<std::string, std::shared_ptr<MotorDriver>> motor_drivers_;
+  std::map<std::string, std::shared_ptr<MotorDriverInterface>> motor_drivers_;
 };
 
-class RoboteqMotorDriver : public MotorDriver
+class RoboteqMotorDriver : public MotorDriverInterface
 {
 public:
-  RoboteqMotorDriver(std::shared_ptr<RoboteqDriver> driver, const std::uint8_t channel)
+  RoboteqMotorDriver(std::weak_ptr<RoboteqDriver> driver, const std::uint8_t channel)
   : driver_(driver), channel_(channel)
   {
   }
@@ -170,7 +171,7 @@ class RoboteqMotorDriver : public MotorDriver
   /**
    * @brief Reads motor state data and saves last timestamps
    */
-  MotorDriverState ReadMotorDriverState() override;
+  MotorDriverState ReadState() override;
 
   /**
    * @brief Sends commands to the motors
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
index da2e9bca..a43e75cd 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
@@ -54,7 +54,7 @@ struct MotorChannels
  * commands. Data is converted between raw Roboteq formats and SI units using
  * roboteq_data_converters.
  */
-class RoboteqRobotDriver : public RobotDriver
+class RoboteqRobotDriver : public RobotDriverInterface
 {
 public:
   RoboteqRobotDriver(
@@ -122,14 +122,6 @@ class RoboteqRobotDriver : public RobotDriver
    */
   void TurnOffEStop() override;
 
-  /**
-   * @brief Turns on Roboteq safety stop. To turn it off, it is necessary to send
-   * 0 commands to motors.
-   *
-   * @exception std::runtime_error if any operation returns error
-   */
-  void TurnOnSafetyStop() override;
-
   /**
    * @brief Get data feedback from the driver
    *
@@ -154,7 +146,7 @@ class RoboteqRobotDriver : public RobotDriver
   DrivetrainSettings drivetrain_settings_;
 
   CANopenManager canopen_manager_;
-  std::map<std::string, std::shared_ptr<Driver>> drivers_;
+  std::map<std::string, std::shared_ptr<DriverInterface>> drivers_;
 
 private:
   void SetMotorsStates(
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp
index 191f9162..56bed72d 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp
@@ -55,7 +55,7 @@ struct RoboteqCANObjects
   static constexpr CANopenObject turn_on_safety_stop = {0x202C, 0};  // Cmd_SFT
 };
 
-MotorDriverState RoboteqMotorDriver::ReadMotorDriverState()
+MotorDriverState RoboteqMotorDriver::ReadState()
 {
   MotorDriverState state;
 
@@ -112,7 +112,7 @@ std::future<void> RoboteqDriver::Boot()
   return future;
 }
 
-DriverState RoboteqDriver::ReadDriverState()
+DriverState RoboteqDriver::ReadState()
 {
   DriverState state;
 
@@ -177,7 +177,7 @@ void RoboteqDriver::TurnOffEStop()
 }
 
 void RoboteqDriver::AddMotorDriver(
-  const std::string name, std::shared_ptr<MotorDriver> motor_driver)
+  const std::string name, std::shared_ptr<MotorDriverInterface> motor_driver)
 {
   if (std::dynamic_pointer_cast<RoboteqMotorDriver>(motor_driver) == nullptr) {
     throw std::runtime_error("Motor driver is not of type RoboteqMotorDriver");
@@ -185,7 +185,7 @@ void RoboteqDriver::AddMotorDriver(
   motor_drivers_.emplace(name, motor_driver);
 }
 
-std::shared_ptr<MotorDriver> RoboteqDriver::GetMotorDriver(const std::string & name)
+std::shared_ptr<MotorDriverInterface> RoboteqDriver::GetMotorDriver(const std::string & name)
 {
   auto it = motor_drivers_.find(name);
   if (it == motor_drivers_.end()) {
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
index 66e6e336..a4ccad8b 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
@@ -114,8 +114,8 @@ void RoboteqRobotDriver::UpdateMotorsState()
   clock_gettime(CLOCK_MONOTONIC, &current_time);
 
   for (auto & [name, driver] : drivers_) {
-    const auto left_state = driver->GetMotorDriver(MotorNames::LEFT)->ReadMotorDriverState();
-    const auto right_state = driver->GetMotorDriver(MotorNames::RIGHT)->ReadMotorDriverState();
+    const auto left_state = driver->GetMotorDriver(MotorNames::LEFT)->ReadState();
+    const auto right_state = driver->GetMotorDriver(MotorNames::RIGHT)->ReadState();
 
     SetMotorsStates(data_.at(name), left_state, right_state, current_time);
   }
@@ -140,7 +140,7 @@ void RoboteqRobotDriver::UpdateDriversState()
   clock_gettime(CLOCK_MONOTONIC, &current_time);
 
   for (auto & [name, driver] : drivers_) {
-    SetDriverState(data_.at(name), driver->ReadDriverState(), current_time);
+    SetDriverState(data_.at(name), driver->ReadState(), current_time);
   }
 
   UpdateCommunicationState();
@@ -190,19 +190,6 @@ void RoboteqRobotDriver::TurnOffEStop()
   }
 }
 
-void RoboteqRobotDriver::TurnOnSafetyStop()
-{
-  for (auto & [name, driver] : drivers_) {
-    try {
-      driver->GetMotorDriver(MotorNames::LEFT)->TurnOnSafetyStop();
-      driver->GetMotorDriver(MotorNames::RIGHT)->TurnOnSafetyStop();
-    } catch (const std::runtime_error & e) {
-      throw std::runtime_error(
-        "Failed to turn on safety stop on " + name + " driver: " + std::string(e.what()));
-    }
-  }
-}
-
 void RoboteqRobotDriver::SetMotorsStates(
   RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
   const timespec & current_time)
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
index 21c6dfc1..549e0887 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
@@ -154,9 +154,9 @@ TEST_F(TestLynxRobotDriver, SendSpeedCommandsInvalidVectorSize)
     [&]() { robot_driver_->SendSpeedCommands(speeds); }, "Invalid speeds vector size"));
 }
 
-TEST_F(TestLynxRobotDriver, AttemptErrorFlagResetWithZeroSpeed)
+TEST_F(TestLynxRobotDriver, AttemptErrorFlagReset)
 {
-  EXPECT_NO_THROW(robot_driver_->AttemptErrorFlagResetWithZeroSpeed());
+  EXPECT_NO_THROW(robot_driver_->AttemptErrorFlagReset());
 }
 
 int main(int argc, char ** argv)
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
index bdd9c5ca..6762b94f 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
@@ -204,9 +204,9 @@ TEST_F(TestPantherRobotDriver, SendSpeedCommandsInvalidVectorSize)
     [&]() { robot_driver_->SendSpeedCommands(speeds); }, "Invalid speeds vector size"));
 }
 
-TEST_F(TestPantherRobotDriver, AttemptErrorFlagResetWithZeroSpeed)
+TEST_F(TestPantherRobotDriver, AttemptErrorFlagReset)
 {
-  EXPECT_NO_THROW(robot_driver_->AttemptErrorFlagResetWithZeroSpeed());
+  EXPECT_NO_THROW(robot_driver_->AttemptErrorFlagReset());
 }
 
 int main(int argc, char ** argv)
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
index a08da907..288bc3d0 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
@@ -153,9 +153,9 @@ TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
   std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
   panther_hardware_interfaces::MotorDriverState motor_driver_state_1 =
-    roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadMotorDriverState();
+    roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadState();
   panther_hardware_interfaces::MotorDriverState motor_driver_state_2 =
-    roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadMotorDriverState();
+    roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadState();
 
   EXPECT_EQ(motor_driver_state_1.pos, motor_1_pos);
   EXPECT_EQ(motor_driver_state_1.vel, motor_1_vel);
@@ -171,13 +171,13 @@ TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps)
   BootRoboteqDriver();
 
   panther_hardware_interfaces::MotorDriverState motor_driver_state_1 =
-    roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadMotorDriverState();
+    roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadState();
 
   // based on publishing frequency in the Roboteq mock (100Hz)
   std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
   panther_hardware_interfaces::MotorDriverState motor_driver_state_2 =
-    roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadMotorDriverState();
+    roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadState();
 
   // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
   // if consecutive messages will have timestamps 100ms + some threshold apart
@@ -202,7 +202,7 @@ TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps)
     std::chrono::milliseconds(5));
 }
 
-TEST_F(TestRoboteqDriver, ReadDriverState)
+TEST_F(TestRoboteqDriver, ReadState)
 {
   using panther_hardware_interfaces_test::DriverChannel;
   using panther_hardware_interfaces_test::DriverFaultFlags;
@@ -230,7 +230,7 @@ TEST_F(TestRoboteqDriver, ReadDriverState)
 
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  panther_hardware_interfaces::DriverState driver_state = roboteq_driver_->ReadDriverState();
+  panther_hardware_interfaces::DriverState driver_state = roboteq_driver_->ReadState();
 
   EXPECT_EQ(driver_state.mcu_temp, temp);
   EXPECT_EQ(driver_state.heatsink_temp, heatsink_temp);
@@ -244,16 +244,16 @@ TEST_F(TestRoboteqDriver, ReadDriverState)
   EXPECT_EQ(driver_state.runtime_stat_flag_channel_2, 0b00001000);
 }
 
-TEST_F(TestRoboteqDriver, ReadDriverStateTimestamp)
+TEST_F(TestRoboteqDriver, ReadStateTimestamp)
 {
   BootRoboteqDriver();
 
-  panther_hardware_interfaces::DriverState driver_state_1 = roboteq_driver_->ReadDriverState();
+  panther_hardware_interfaces::DriverState driver_state_1 = roboteq_driver_->ReadState();
 
   // based on publishing frequency in the Roboteq mock (20Hz)
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  panther_hardware_interfaces::DriverState driver_state_2 = roboteq_driver_->ReadDriverState();
+  panther_hardware_interfaces::DriverState driver_state_2 = roboteq_driver_->ReadState();
 
   // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
   // if consecutive messages will have timestamps 100ms + some threshold apart
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
index 33ffb06b..9a5cdb13 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
@@ -43,89 +43,52 @@ class RoboteqRobotDriverWrapper : public panther_hardware_interfaces::RoboteqRob
   : RoboteqRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time)
   {
     // Assume 2 drivers and 4 motor drivers
-    mock_fl_motor_driver_ =
+    mock_fl_motor_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
-    mock_fr_motor_driver_ =
+    mock_fr_motor_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
-    mock_rl_motor_driver_ =
+    mock_rl_motor_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
-    mock_rr_motor_driver_ =
+    mock_rr_motor_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
 
-    mock_front_driver_ =
+    mock_front_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
-    mock_front_driver_->AddMotorDriver(kLeftMotorDriverName, mock_fl_motor_driver_);
-    mock_front_driver_->AddMotorDriver(kRightMotorDriverName, mock_fr_motor_driver_);
+    mock_front_driver->AddMotorDriver(kLeftMotorDriverName, mock_fl_motor_driver);
+    mock_front_driver->AddMotorDriver(kRightMotorDriverName, mock_fr_motor_driver);
 
-    mock_rear_driver_ =
+    mock_rear_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
-    mock_rear_driver_->AddMotorDriver(kLeftMotorDriverName, mock_rl_motor_driver_);
-    mock_rear_driver_->AddMotorDriver(kRightMotorDriverName, mock_rr_motor_driver_);
+    mock_rear_driver->AddMotorDriver(kLeftMotorDriverName, mock_rl_motor_driver);
+    mock_rear_driver->AddMotorDriver(kRightMotorDriverName, mock_rr_motor_driver);
   }
 
   void DefineDrivers() override
   {
-    drivers_.emplace(kFrontDriverName, mock_front_driver_);
-    drivers_.emplace(kRearDriverName, mock_rear_driver_);
+    drivers_.emplace(kFrontDriverName, mock_front_driver);
+    drivers_.emplace(kRearDriverName, mock_rear_driver);
   }
 
   void SendSpeedCommands(const std::vector<float> & /*velocities*/) override {}
-  void AttemptErrorFlagResetWithZeroSpeed() override {}
-
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
-  GetMockFrontDriver()
-  {
-    return mock_front_driver_;
-  }
-
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
-  GetMockRearDriver()
-  {
-    return mock_rear_driver_;
-  }
-
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-  GetMockFLMotorDriver()
-  {
-    return mock_fl_motor_driver_;
-  }
-
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-  GetMockFRMotorDriver()
-  {
-    return mock_fr_motor_driver_;
-  }
-
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-  GetMockRLMotorDriver()
-  {
-    return mock_rl_motor_driver_;
-  }
-
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-  GetMockRRMotorDriver()
-  {
-    return mock_rr_motor_driver_;
-  }
+  void AttemptErrorFlagReset() override {}
 
   static constexpr char kFrontDriverName[] = "front";
   static constexpr char kRearDriverName[] = "rear";
   static constexpr char kLeftMotorDriverName[] = "left";
   static constexpr char kRightMotorDriverName[] = "right";
 
-private:
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
-    mock_front_driver_;
+    mock_front_driver;
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
-    mock_rear_driver_;
+    mock_rear_driver;
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-    mock_fl_motor_driver_;
+    mock_fl_motor_driver;
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-    mock_fr_motor_driver_;
+    mock_fr_motor_driver;
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-    mock_rl_motor_driver_;
+    mock_rl_motor_driver;
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-    mock_rr_motor_driver_;
+    mock_rr_motor_driver;
 };
 
 class TestPantherRobotDriverInitialization : public ::testing::Test
@@ -159,30 +122,40 @@ class TestPantherRobotDriver : public TestPantherRobotDriverInitialization
   }
 
   ~TestPantherRobotDriver() { robot_driver_->Deinitialize(); }
+
+  timespec GetCurrentTimeWithTimeout(const std::chrono::nanoseconds & timeout_ns)
+  {
+    timespec current_time;
+    clock_gettime(CLOCK_MONOTONIC, &current_time);
+
+    current_time.tv_nsec -= timeout_ns.count();
+
+    return current_time;
+  }
 };
 
 TEST_F(TestPantherRobotDriverInitialization, Initialize)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), Boot()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), Boot()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_front_driver, Boot()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, Boot()).Times(1);
 
   EXPECT_NO_THROW(robot_driver_->Initialize());
   ASSERT_NO_THROW(robot_driver_->Deinitialize());
 
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), Boot()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), Boot()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_front_driver, Boot()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, Boot()).Times(1);
   // Check if deinitialization worked correctly - initialize once again
   EXPECT_NO_THROW(robot_driver_->Initialize());
 }
 
 TEST_F(TestPantherRobotDriverInitialization, Activate)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), ResetScript()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), ResetScript()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockFLMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockFRMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRLMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRRMotorDriver(), SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_front_driver, ResetScript()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, ResetScript()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_fl_motor_driver, SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_fr_motor_driver, SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_rl_motor_driver, SendCmdVel(::testing::Eq(0))).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_rr_motor_driver, SendCmdVel(::testing::Eq(0))).Times(1);
 
   ASSERT_NO_THROW(robot_driver_->Initialize());
   ASSERT_NO_THROW(robot_driver_->Activate());
@@ -205,18 +178,18 @@ TEST_F(TestPantherRobotDriver, GetDataError)
 
 TEST_F(TestPantherRobotDriver, UpdateCommunicationState)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsCANError()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsHeartbeatTimeout()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsCANError()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsHeartbeatTimeout()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_front_driver, IsHeartbeatTimeout()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, IsCANError()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, IsHeartbeatTimeout()).Times(1);
 
   ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
 }
 
 TEST_F(TestPantherRobotDriver, UpdateCommunicationStateCANErorr)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsCANError()).WillOnce(::testing::Return(true));
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsCANError()).WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, IsCANError()).WillOnce(::testing::Return(true));
 
   ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
 
@@ -226,9 +199,9 @@ TEST_F(TestPantherRobotDriver, UpdateCommunicationStateCANErorr)
 
 TEST_F(TestPantherRobotDriver, UpdateCommunicationStateHeartbeatTimeout)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsHeartbeatTimeout())
+  EXPECT_CALL(*robot_driver_->mock_front_driver, IsHeartbeatTimeout())
     .WillOnce(::testing::Return(true));
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsHeartbeatTimeout())
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, IsHeartbeatTimeout())
     .WillOnce(::testing::Return(true));
 
   ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
@@ -259,16 +232,16 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsState)
   const std::int32_t rr_vel = 402;
   const std::int32_t rr_current = 403;
 
-  ON_CALL(*robot_driver_->GetMockFLMotorDriver(), ReadMotorDriverState())
+  ON_CALL(*robot_driver_->mock_fl_motor_driver, ReadState())
     .WillByDefault(::testing::Return(
       panther_hardware_interfaces::MotorDriverState({fl_pos, fl_vel, fl_current, {0, 0}, {0, 0}})));
-  ON_CALL(*robot_driver_->GetMockFRMotorDriver(), ReadMotorDriverState())
+  ON_CALL(*robot_driver_->mock_fr_motor_driver, ReadState())
     .WillByDefault(::testing::Return(
       panther_hardware_interfaces::MotorDriverState({fr_pos, fr_vel, fr_current, {0, 0}, {0, 0}})));
-  ON_CALL(*robot_driver_->GetMockRLMotorDriver(), ReadMotorDriverState())
+  ON_CALL(*robot_driver_->mock_rl_motor_driver, ReadState())
     .WillByDefault(::testing::Return(
       panther_hardware_interfaces::MotorDriverState({rl_pos, rl_vel, rl_current, {0, 0}, {0, 0}})));
-  ON_CALL(*robot_driver_->GetMockRRMotorDriver(), ReadMotorDriverState())
+  ON_CALL(*robot_driver_->mock_rr_motor_driver, ReadState())
     .WillByDefault(::testing::Return(
       panther_hardware_interfaces::MotorDriverState({rr_pos, rr_vel, rr_current, {0, 0}, {0, 0}})));
 
@@ -302,28 +275,29 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsState)
 
 TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimestamps)
 {
-  auto read_motor_driver_state_method = []() {
+  auto current_time = GetCurrentTimeWithTimeout(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms);
+
+  auto read_motor_driver_state_method = [&current_time]() {
     panther_hardware_interfaces::MotorDriverState state;
-    clock_gettime(CLOCK_MONOTONIC, &state.pos_timestamp);
-    clock_gettime(CLOCK_MONOTONIC, &state.vel_current_timestamp);
+    state.pos_timestamp = current_time;
+    state.vel_current_timestamp = current_time;
     return state;
   };
 
-  ON_CALL(*robot_driver_->GetMockFLMotorDriver(), ReadMotorDriverState())
+  ON_CALL(*robot_driver_->mock_fl_motor_driver, ReadState())
     .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
-  ON_CALL(*robot_driver_->GetMockFRMotorDriver(), ReadMotorDriverState())
+  ON_CALL(*robot_driver_->mock_fr_motor_driver, ReadState())
     .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
-  ON_CALL(*robot_driver_->GetMockRLMotorDriver(), ReadMotorDriverState())
+  ON_CALL(*robot_driver_->mock_rl_motor_driver, ReadState())
     .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
-  ON_CALL(*robot_driver_->GetMockRRMotorDriver(), ReadMotorDriverState())
+  ON_CALL(*robot_driver_->mock_rr_motor_driver, ReadState())
     .WillByDefault(::testing::Invoke(read_motor_driver_state_method));
 
   robot_driver_->UpdateMotorsState();
 
-  // sleep for timeout and check if timestamps were updated correctly
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
-    std::chrono::milliseconds(10));
+  // Update current time to exceed timeout
+  clock_gettime(CLOCK_MONOTONIC, &current_time);
 
   robot_driver_->UpdateMotorsState();
 
@@ -335,25 +309,20 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimestamps)
 
 TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimeout)
 {
-  timespec current_time;
-  clock_gettime(CLOCK_MONOTONIC, &current_time);
+  const auto current_time = GetCurrentTimeWithTimeout(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms);
 
   panther_hardware_interfaces::MotorDriverState state = {0, 0, 0, current_time, current_time};
 
-  ON_CALL(*robot_driver_->GetMockFLMotorDriver(), ReadMotorDriverState())
+  ON_CALL(*robot_driver_->mock_fl_motor_driver, ReadState())
     .WillByDefault(::testing::Return(state));
-  ON_CALL(*robot_driver_->GetMockFRMotorDriver(), ReadMotorDriverState())
+  ON_CALL(*robot_driver_->mock_fr_motor_driver, ReadState())
     .WillByDefault(::testing::Return(state));
-  ON_CALL(*robot_driver_->GetMockRLMotorDriver(), ReadMotorDriverState())
+  ON_CALL(*robot_driver_->mock_rl_motor_driver, ReadState())
     .WillByDefault(::testing::Return(state));
-  ON_CALL(*robot_driver_->GetMockRRMotorDriver(), ReadMotorDriverState())
+  ON_CALL(*robot_driver_->mock_rr_motor_driver, ReadState())
     .WillByDefault(::testing::Return(state));
 
-  // sleep for pdo_motor_states_timeout_ms + 10ms
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms +
-    std::chrono::milliseconds(10));
-
   robot_driver_->UpdateMotorsState();
 
   EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName)
@@ -388,32 +357,38 @@ TEST_F(TestPantherRobotDriver, UpdateDriverState)
   const std::uint8_t runtime_error_forward_limit_triggered = static_cast<std::uint8_t>(0b10000);
   const std::uint8_t runtime_error_reverse_limit_triggered = static_cast<std::uint8_t>(0b100000);
 
-  ON_CALL(*robot_driver_->GetMockFrontDriver(), ReadDriverState())
-    .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
-      {fault_flag_overheat,
-       script_flag_encoder_disconnected,
-       runtime_error_loop_error,
-       runtime_error_safety_stop_active,
-       f_battery_current_1,
-       f_battery_current_2,
-       f_volt,
-       f_temp,
-       f_heatsink_temp,
-       {0, 0},
-       {0, 0}})));
-  ON_CALL(*robot_driver_->GetMockRearDriver(), ReadDriverState())
-    .WillByDefault(::testing::Return(panther_hardware_interfaces::DriverState(
-      {fault_flag_overvoltage,
-       script_flag_amp_limiter,
-       runtime_error_forward_limit_triggered,
-       runtime_error_reverse_limit_triggered,
-       r_battery_current_1,
-       r_battery_current_2,
-       r_volt,
-       r_temp,
-       r_heatsink_temp,
-       {0, 0},
-       {0, 0}})));
+  panther_hardware_interfaces::DriverState front_driver_state_data = {
+    fault_flag_overheat,
+    script_flag_encoder_disconnected,
+    runtime_error_loop_error,
+    runtime_error_safety_stop_active,
+    f_battery_current_1,
+    f_battery_current_2,
+    f_volt,
+    f_temp,
+    f_heatsink_temp,
+    {0, 0},
+    {0, 0}};
+
+  panther_hardware_interfaces::DriverState rear_driver_state_data = {
+    fault_flag_overvoltage,
+    script_flag_amp_limiter,
+    runtime_error_forward_limit_triggered,
+    runtime_error_reverse_limit_triggered,
+    r_battery_current_1,
+    r_battery_current_2,
+    r_volt,
+    r_temp,
+    r_heatsink_temp,
+    {0, 0},
+    {0, 0}};
+
+  ON_CALL(*robot_driver_->mock_front_driver, ReadState())
+    .WillByDefault(
+      ::testing::Return(panther_hardware_interfaces::DriverState(front_driver_state_data)));
+  ON_CALL(*robot_driver_->mock_rear_driver, ReadState())
+    .WillByDefault(
+      ::testing::Return(panther_hardware_interfaces::DriverState(rear_driver_state_data)));
 
   robot_driver_->UpdateDriversState();
 
@@ -452,24 +427,25 @@ TEST_F(TestPantherRobotDriver, UpdateDriverState)
 
 TEST_F(TestPantherRobotDriver, UpdateDriverStateTimestamps)
 {
-  auto read_driver_state_method = []() {
+  auto current_time = GetCurrentTimeWithTimeout(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms);
+
+  auto read_driver_state_method = [&current_time]() {
     panther_hardware_interfaces::DriverState state;
-    clock_gettime(CLOCK_MONOTONIC, &state.flags_current_timestamp);
-    clock_gettime(CLOCK_MONOTONIC, &state.voltages_temps_timestamp);
+    state.flags_current_timestamp = current_time;
+    state.voltages_temps_timestamp = current_time;
     return state;
   };
 
-  ON_CALL(*robot_driver_->GetMockFrontDriver(), ReadDriverState())
+  ON_CALL(*robot_driver_->mock_front_driver, ReadState())
     .WillByDefault(::testing::Invoke(read_driver_state_method));
-  ON_CALL(*robot_driver_->GetMockRearDriver(), ReadDriverState())
+  ON_CALL(*robot_driver_->mock_rear_driver, ReadState())
     .WillByDefault(::testing::Invoke(read_driver_state_method));
 
   robot_driver_->UpdateDriversState();
 
-  // sleep for timeout and check if timestamps were updated correctly
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
-    std::chrono::milliseconds(10));
+  // Update current time to exceed timeout
+  clock_gettime(CLOCK_MONOTONIC, &current_time);
 
   robot_driver_->UpdateDriversState();
 
@@ -481,21 +457,14 @@ TEST_F(TestPantherRobotDriver, UpdateDriverStateTimestamps)
 
 TEST_F(TestPantherRobotDriver, UpdateDriverStateTimeout)
 {
-  timespec current_time;
-  clock_gettime(CLOCK_MONOTONIC, &current_time);
+  const auto current_time = GetCurrentTimeWithTimeout(
+    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms);
 
   panther_hardware_interfaces::DriverState state = {
     0, 0, 0, 0, 0, 0, 0, 0, 0, current_time, current_time};
 
-  ON_CALL(*robot_driver_->GetMockFrontDriver(), ReadDriverState())
-    .WillByDefault(::testing::Return(state));
-  ON_CALL(*robot_driver_->GetMockRearDriver(), ReadDriverState())
-    .WillByDefault(::testing::Return(state));
-
-  // sleep for pdo_driver_state_timeout_ms + 10ms
-  std::this_thread::sleep_for(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms +
-    std::chrono::milliseconds(10));
+  ON_CALL(*robot_driver_->mock_front_driver, ReadState()).WillByDefault(::testing::Return(state));
+  ON_CALL(*robot_driver_->mock_rear_driver, ReadState()).WillByDefault(::testing::Return(state));
 
   robot_driver_->UpdateDriversState();
 
@@ -509,59 +478,38 @@ TEST_F(TestPantherRobotDriver, UpdateDriverStateTimeout)
 
 TEST_F(TestPantherRobotDriver, TurnOnEStop)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOnEStop()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOnEStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_front_driver, TurnOnEStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, TurnOnEStop()).Times(1);
 
   EXPECT_NO_THROW(robot_driver_->TurnOnEStop());
 }
 
 TEST_F(TestPantherRobotDriver, TurnOffEStop)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOffEStop()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOffEStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_front_driver, TurnOffEStop()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, TurnOffEStop()).Times(1);
 
   EXPECT_NO_THROW(robot_driver_->TurnOffEStop());
 }
 
 TEST_F(TestPantherRobotDriver, TurnOnEStopError)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOnEStop())
+  EXPECT_CALL(*robot_driver_->mock_front_driver, TurnOnEStop())
     .WillOnce(::testing::Throw(std::runtime_error("")));
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOnEStop()).Times(0);
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, TurnOnEStop()).Times(0);
 
   EXPECT_THROW(robot_driver_->TurnOnEStop(), std::runtime_error);
 }
 
 TEST_F(TestPantherRobotDriver, TurnOffEStopError)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), TurnOffEStop())
+  EXPECT_CALL(*robot_driver_->mock_front_driver, TurnOffEStop())
     .WillOnce(::testing::Throw(std::runtime_error("")));
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), TurnOffEStop()).Times(0);
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, TurnOffEStop()).Times(0);
 
   EXPECT_THROW(robot_driver_->TurnOffEStop(), std::runtime_error);
 }
 
-TEST_F(TestPantherRobotDriver, SafetyStop)
-{
-  EXPECT_CALL(*robot_driver_->GetMockFLMotorDriver(), TurnOnSafetyStop()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockFRMotorDriver(), TurnOnSafetyStop()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRLMotorDriver(), TurnOnSafetyStop()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRRMotorDriver(), TurnOnSafetyStop()).Times(1);
-
-  EXPECT_NO_THROW(robot_driver_->TurnOnSafetyStop());
-}
-
-TEST_F(TestPantherRobotDriver, SafetyStopError)
-{
-  EXPECT_CALL(*robot_driver_->GetMockFLMotorDriver(), TurnOnSafetyStop())
-    .WillOnce(::testing::Throw(std::runtime_error("")));
-  EXPECT_CALL(*robot_driver_->GetMockFRMotorDriver(), TurnOnSafetyStop()).Times(0);
-  EXPECT_CALL(*robot_driver_->GetMockRLMotorDriver(), TurnOnSafetyStop()).Times(0);
-  EXPECT_CALL(*robot_driver_->GetMockRRMotorDriver(), TurnOnSafetyStop()).Times(0);
-
-  EXPECT_THROW(robot_driver_->TurnOnSafetyStop(), std::runtime_error);
-}
-
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/panther_hardware_interfaces/test/utils/mock_driver.hpp b/panther_hardware_interfaces/test/utils/mock_driver.hpp
index eb354a63..7af1cca1 100644
--- a/panther_hardware_interfaces/test/utils/mock_driver.hpp
+++ b/panther_hardware_interfaces/test/utils/mock_driver.hpp
@@ -27,19 +27,19 @@
 namespace panther_hardware_interfaces_test
 {
 
-class MockDriver : public panther_hardware_interfaces::Driver
+class MockDriver : public panther_hardware_interfaces::DriverInterface
 {
 public:
   MOCK_METHOD(std::future<void>, Boot, (), (override));
   MOCK_METHOD(bool, IsCANError, (), (const, override));
   MOCK_METHOD(bool, IsHeartbeatTimeout, (), (const, override));
 
-  MOCK_METHOD(panther_hardware_interfaces::DriverState, ReadDriverState, (), (override));
+  MOCK_METHOD(panther_hardware_interfaces::DriverState, ReadState, (), (override));
   MOCK_METHOD(void, ResetScript, (), (override));
   MOCK_METHOD(void, TurnOnEStop, (), (override));
   MOCK_METHOD(void, TurnOffEStop, (), (override));
 
-  std::shared_ptr<panther_hardware_interfaces::MotorDriver> GetMotorDriver(
+  std::shared_ptr<panther_hardware_interfaces::MotorDriverInterface> GetMotorDriver(
     const std::string & name) override
   {
     return motor_drivers_.at(name);
@@ -47,19 +47,20 @@ class MockDriver : public panther_hardware_interfaces::Driver
 
   void AddMotorDriver(
     const std::string name,
-    std::shared_ptr<panther_hardware_interfaces::MotorDriver> motor_driver) override
+    std::shared_ptr<panther_hardware_interfaces::MotorDriverInterface> motor_driver) override
   {
     motor_drivers_.emplace(name, motor_driver);
   }
 
 private:
-  std::map<std::string, std::shared_ptr<panther_hardware_interfaces::MotorDriver>> motor_drivers_;
+  std::map<std::string, std::shared_ptr<panther_hardware_interfaces::MotorDriverInterface>>
+    motor_drivers_;
 };
 
-class MockMotorDriver : public panther_hardware_interfaces::MotorDriver
+class MockMotorDriver : public panther_hardware_interfaces::MotorDriverInterface
 {
 public:
-  MOCK_METHOD(panther_hardware_interfaces::MotorDriverState, ReadMotorDriverState, (), (override));
+  MOCK_METHOD(panther_hardware_interfaces::MotorDriverState, ReadState, (), (override));
   MOCK_METHOD(void, SendCmdVel, (const std::int32_t cmd), (override));
   MOCK_METHOD(void, TurnOnSafetyStop, (), (override));
 };

From ae591b379a042a9719d38e45f815991eb32025be Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Wed, 11 Sep 2024 06:21:27 +0000
Subject: [PATCH 042/100] fix test name

---
 .../test_roboteq_robot_driver.cpp             | 46 +++++++++----------
 1 file changed, 23 insertions(+), 23 deletions(-)

diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
index 9a5cdb13..9c990a7f 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
@@ -91,10 +91,10 @@ class RoboteqRobotDriverWrapper : public panther_hardware_interfaces::RoboteqRob
     mock_rr_motor_driver;
 };
 
-class TestPantherRobotDriverInitialization : public ::testing::Test
+class TestRoboteqRobotDriverInitialization : public ::testing::Test
 {
 public:
-  TestPantherRobotDriverInitialization()
+  TestRoboteqRobotDriverInitialization()
   {
     can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
       panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
@@ -105,23 +105,23 @@ class TestPantherRobotDriverInitialization : public ::testing::Test
       panther_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10));
   }
 
-  ~TestPantherRobotDriverInitialization() {}
+  ~TestRoboteqRobotDriverInitialization() {}
 
 protected:
   std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
   std::unique_ptr<RoboteqRobotDriverWrapper> robot_driver_;
 };
 
-class TestPantherRobotDriver : public TestPantherRobotDriverInitialization
+class TestRoboteqRobotDriver : public TestRoboteqRobotDriverInitialization
 {
 public:
-  TestPantherRobotDriver()
+  TestRoboteqRobotDriver()
   {
     robot_driver_->Initialize();
     robot_driver_->Activate();
   }
 
-  ~TestPantherRobotDriver() { robot_driver_->Deinitialize(); }
+  ~TestRoboteqRobotDriver() { robot_driver_->Deinitialize(); }
 
   timespec GetCurrentTimeWithTimeout(const std::chrono::nanoseconds & timeout_ns)
   {
@@ -134,7 +134,7 @@ class TestPantherRobotDriver : public TestPantherRobotDriverInitialization
   }
 };
 
-TEST_F(TestPantherRobotDriverInitialization, Initialize)
+TEST_F(TestRoboteqRobotDriverInitialization, Initialize)
 {
   EXPECT_CALL(*robot_driver_->mock_front_driver, Boot()).Times(1);
   EXPECT_CALL(*robot_driver_->mock_rear_driver, Boot()).Times(1);
@@ -148,7 +148,7 @@ TEST_F(TestPantherRobotDriverInitialization, Initialize)
   EXPECT_NO_THROW(robot_driver_->Initialize());
 }
 
-TEST_F(TestPantherRobotDriverInitialization, Activate)
+TEST_F(TestRoboteqRobotDriverInitialization, Activate)
 {
   EXPECT_CALL(*robot_driver_->mock_front_driver, ResetScript()).Times(1);
   EXPECT_CALL(*robot_driver_->mock_rear_driver, ResetScript()).Times(1);
@@ -161,13 +161,13 @@ TEST_F(TestPantherRobotDriverInitialization, Activate)
   ASSERT_NO_THROW(robot_driver_->Activate());
 }
 
-TEST_F(TestPantherRobotDriver, GetData)
+TEST_F(TestRoboteqRobotDriver, GetData)
 {
   EXPECT_NO_THROW(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName));
   EXPECT_NO_THROW(robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName));
 }
 
-TEST_F(TestPantherRobotDriver, GetDataError)
+TEST_F(TestRoboteqRobotDriver, GetDataError)
 {
   const std::string name = "invalid_name";
   const std::string error_msg = "Data with name '" + name + "' does not exist.";
@@ -176,7 +176,7 @@ TEST_F(TestPantherRobotDriver, GetDataError)
     [&] { robot_driver_->GetData(name); }, error_msg));
 }
 
-TEST_F(TestPantherRobotDriver, UpdateCommunicationState)
+TEST_F(TestRoboteqRobotDriver, UpdateCommunicationState)
 {
   EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).Times(1);
   EXPECT_CALL(*robot_driver_->mock_front_driver, IsHeartbeatTimeout()).Times(1);
@@ -186,7 +186,7 @@ TEST_F(TestPantherRobotDriver, UpdateCommunicationState)
   ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
 }
 
-TEST_F(TestPantherRobotDriver, UpdateCommunicationStateCANErorr)
+TEST_F(TestRoboteqRobotDriver, UpdateCommunicationStateCANErorr)
 {
   EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).WillOnce(::testing::Return(true));
   EXPECT_CALL(*robot_driver_->mock_rear_driver, IsCANError()).WillOnce(::testing::Return(true));
@@ -197,7 +197,7 @@ TEST_F(TestPantherRobotDriver, UpdateCommunicationStateCANErorr)
   EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsCANError());
 }
 
-TEST_F(TestPantherRobotDriver, UpdateCommunicationStateHeartbeatTimeout)
+TEST_F(TestRoboteqRobotDriver, UpdateCommunicationStateHeartbeatTimeout)
 {
   EXPECT_CALL(*robot_driver_->mock_front_driver, IsHeartbeatTimeout())
     .WillOnce(::testing::Return(true));
@@ -212,7 +212,7 @@ TEST_F(TestPantherRobotDriver, UpdateCommunicationStateHeartbeatTimeout)
     robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsHeartbeatTimeout());
 }
 
-TEST_F(TestPantherRobotDriver, UpdateMotorsState)
+TEST_F(TestRoboteqRobotDriver, UpdateMotorsState)
 {
   using panther_hardware_interfaces::MotorChannels;
   using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
@@ -273,7 +273,7 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsState)
   EXPECT_FLOAT_EQ(rr_data.GetTorque(), rr_current * kRbtqCurrentFbToNewtonMeters);
 }
 
-TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimestamps)
+TEST_F(TestRoboteqRobotDriver, UpdateMotorsStateTimestamps)
 {
   auto current_time = GetCurrentTimeWithTimeout(
     panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms);
@@ -307,7 +307,7 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimestamps)
     robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsMotorStatesDataTimedOut());
 }
 
-TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimeout)
+TEST_F(TestRoboteqRobotDriver, UpdateMotorsStateTimeout)
 {
   const auto current_time = GetCurrentTimeWithTimeout(
     panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms);
@@ -333,7 +333,7 @@ TEST_F(TestPantherRobotDriver, UpdateMotorsStateTimeout)
   EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsError());
 }
 
-TEST_F(TestPantherRobotDriver, UpdateDriverState)
+TEST_F(TestRoboteqRobotDriver, UpdateDriverState)
 {
   using panther_hardware_interfaces::MotorChannels;
 
@@ -425,7 +425,7 @@ TEST_F(TestPantherRobotDriver, UpdateDriverState)
   EXPECT_TRUE(rear_data.GetRuntimeError(MotorChannels::LEFT).GetMessage().reverse_limit_triggered);
 }
 
-TEST_F(TestPantherRobotDriver, UpdateDriverStateTimestamps)
+TEST_F(TestRoboteqRobotDriver, UpdateDriverStateTimestamps)
 {
   auto current_time = GetCurrentTimeWithTimeout(
     panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms);
@@ -455,7 +455,7 @@ TEST_F(TestPantherRobotDriver, UpdateDriverStateTimestamps)
     robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsDriverStateDataTimedOut());
 }
 
-TEST_F(TestPantherRobotDriver, UpdateDriverStateTimeout)
+TEST_F(TestRoboteqRobotDriver, UpdateDriverStateTimeout)
 {
   const auto current_time = GetCurrentTimeWithTimeout(
     panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms);
@@ -476,7 +476,7 @@ TEST_F(TestPantherRobotDriver, UpdateDriverStateTimeout)
   EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsError());
 }
 
-TEST_F(TestPantherRobotDriver, TurnOnEStop)
+TEST_F(TestRoboteqRobotDriver, TurnOnEStop)
 {
   EXPECT_CALL(*robot_driver_->mock_front_driver, TurnOnEStop()).Times(1);
   EXPECT_CALL(*robot_driver_->mock_rear_driver, TurnOnEStop()).Times(1);
@@ -484,7 +484,7 @@ TEST_F(TestPantherRobotDriver, TurnOnEStop)
   EXPECT_NO_THROW(robot_driver_->TurnOnEStop());
 }
 
-TEST_F(TestPantherRobotDriver, TurnOffEStop)
+TEST_F(TestRoboteqRobotDriver, TurnOffEStop)
 {
   EXPECT_CALL(*robot_driver_->mock_front_driver, TurnOffEStop()).Times(1);
   EXPECT_CALL(*robot_driver_->mock_rear_driver, TurnOffEStop()).Times(1);
@@ -492,7 +492,7 @@ TEST_F(TestPantherRobotDriver, TurnOffEStop)
   EXPECT_NO_THROW(robot_driver_->TurnOffEStop());
 }
 
-TEST_F(TestPantherRobotDriver, TurnOnEStopError)
+TEST_F(TestRoboteqRobotDriver, TurnOnEStopError)
 {
   EXPECT_CALL(*robot_driver_->mock_front_driver, TurnOnEStop())
     .WillOnce(::testing::Throw(std::runtime_error("")));
@@ -501,7 +501,7 @@ TEST_F(TestPantherRobotDriver, TurnOnEStopError)
   EXPECT_THROW(robot_driver_->TurnOnEStop(), std::runtime_error);
 }
 
-TEST_F(TestPantherRobotDriver, TurnOffEStopError)
+TEST_F(TestRoboteqRobotDriver, TurnOffEStopError)
 {
   EXPECT_CALL(*robot_driver_->mock_front_driver, TurnOffEStop())
     .WillOnce(::testing::Throw(std::runtime_error("")));

From 8e96e4e9205ebe8f3944a3c0ccb02a4e01221393 Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Wed, 11 Sep 2024 06:29:11 +0000
Subject: [PATCH 043/100] test remove getters

---
 .../robot_driver/test_lynx_robot_driver.cpp   | 50 +++-------
 .../test_panther_robot_driver.cpp             | 97 ++++++-------------
 2 files changed, 46 insertions(+), 101 deletions(-)

diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
index 549e0887..da5706cd 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
@@ -40,47 +40,29 @@ class LynxRobotDriverWrapper : public panther_hardware_interfaces::LynxRobotDriv
     const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000))
   : LynxRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time)
   {
-    mock_left_motor_driver_ =
+    mock_left_motor_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
-    mock_right_motor_driver_ =
+    mock_right_motor_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
 
-    mock_driver_ =
+    mock_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
-    mock_driver_->AddMotorDriver(
-      panther_hardware_interfaces::MotorNames::LEFT, mock_left_motor_driver_);
-    mock_driver_->AddMotorDriver(
-      panther_hardware_interfaces::MotorNames::RIGHT, mock_right_motor_driver_);
+    mock_driver->AddMotorDriver(
+      panther_hardware_interfaces::MotorNames::LEFT, mock_left_motor_driver);
+    mock_driver->AddMotorDriver(
+      panther_hardware_interfaces::MotorNames::RIGHT, mock_right_motor_driver);
   }
 
   void DefineDrivers() override
   {
-    drivers_.emplace(panther_hardware_interfaces::DriverNames::DEFAULT, mock_driver_);
+    drivers_.emplace(panther_hardware_interfaces::DriverNames::DEFAULT, mock_driver);
   }
 
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>> GetMockDriver()
-  {
-    return mock_driver_;
-  }
-
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-  GetMockLeftMotorDriver()
-  {
-    return mock_left_motor_driver_;
-  }
-
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-  GetMockRightMotorDriver()
-  {
-    return mock_right_motor_driver_;
-  }
-
-private:
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>> mock_driver_;
+  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>> mock_driver;
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-    mock_left_motor_driver_;
+    mock_left_motor_driver;
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-    mock_right_motor_driver_;
+    mock_right_motor_driver;
 };
 
 class TestLynxRobotDriver : public ::testing::Test
@@ -114,13 +96,13 @@ TEST_F(TestLynxRobotDriver, SendSpeedCommands)
   const float left_v = 0.1;
   const float right_v = 0.2;
 
-  EXPECT_CALL(*robot_driver_->GetMockDriver(), IsCANError()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_driver, IsCANError()).Times(1);
   EXPECT_CALL(
-    *robot_driver_->GetMockLeftMotorDriver(),
+    *robot_driver_->mock_left_motor_driver,
     SendCmdVel(::testing::Eq(static_cast<std::int32_t>(left_v * kRadPerSecToRbtqCmd))))
     .Times(1);
   EXPECT_CALL(
-    *robot_driver_->GetMockRightMotorDriver(),
+    *robot_driver_->mock_right_motor_driver,
     SendCmdVel(::testing::Eq(static_cast<std::int32_t>(right_v * kRadPerSecToRbtqCmd))))
     .Times(1);
 
@@ -130,7 +112,7 @@ TEST_F(TestLynxRobotDriver, SendSpeedCommands)
 
 TEST_F(TestLynxRobotDriver, SendSpeedCommandsSendCmdVelError)
 {
-  EXPECT_CALL(*robot_driver_->GetMockLeftMotorDriver(), SendCmdVel(::testing::_))
+  EXPECT_CALL(*robot_driver_->mock_left_motor_driver, SendCmdVel(::testing::_))
     .WillOnce(::testing::Throw(std::runtime_error("")));
 
   EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
@@ -139,7 +121,7 @@ TEST_F(TestLynxRobotDriver, SendSpeedCommandsSendCmdVelError)
 
 TEST_F(TestLynxRobotDriver, SendSpeedCommandsCANError)
 {
-  EXPECT_CALL(*robot_driver_->GetMockDriver(), IsCANError()).WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->mock_driver, IsCANError()).WillOnce(::testing::Return(true));
 
   EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { robot_driver_->SendSpeedCommands({0.0, 0.0}); },
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
index 6762b94f..7ba08f5f 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
@@ -40,85 +40,48 @@ class PantherRobotDriverWrapper : public panther_hardware_interfaces::PantherRob
     const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000))
   : PantherRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time)
   {
-    mock_fl_motor_driver_ =
+    mock_fl_motor_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
-    mock_fr_motor_driver_ =
+    mock_fr_motor_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
-    mock_rl_motor_driver_ =
+    mock_rl_motor_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
-    mock_rr_motor_driver_ =
+    mock_rr_motor_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
 
-    mock_front_driver_ =
+    mock_front_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
-    mock_front_driver_->AddMotorDriver(
-      panther_hardware_interfaces::MotorNames::LEFT, mock_fl_motor_driver_);
-    mock_front_driver_->AddMotorDriver(
-      panther_hardware_interfaces::MotorNames::RIGHT, mock_fr_motor_driver_);
+    mock_front_driver->AddMotorDriver(
+      panther_hardware_interfaces::MotorNames::LEFT, mock_fl_motor_driver);
+    mock_front_driver->AddMotorDriver(
+      panther_hardware_interfaces::MotorNames::RIGHT, mock_fr_motor_driver);
 
-    mock_rear_driver_ =
+    mock_rear_driver =
       std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
-    mock_rear_driver_->AddMotorDriver(
-      panther_hardware_interfaces::MotorNames::LEFT, mock_rl_motor_driver_);
-    mock_rear_driver_->AddMotorDriver(
-      panther_hardware_interfaces::MotorNames::RIGHT, mock_rr_motor_driver_);
+    mock_rear_driver->AddMotorDriver(
+      panther_hardware_interfaces::MotorNames::LEFT, mock_rl_motor_driver);
+    mock_rear_driver->AddMotorDriver(
+      panther_hardware_interfaces::MotorNames::RIGHT, mock_rr_motor_driver);
   }
 
   void DefineDrivers() override
   {
-    drivers_.emplace(panther_hardware_interfaces::DriverNames::FRONT, mock_front_driver_);
-    drivers_.emplace(panther_hardware_interfaces::DriverNames::REAR, mock_rear_driver_);
+    drivers_.emplace(panther_hardware_interfaces::DriverNames::FRONT, mock_front_driver);
+    drivers_.emplace(panther_hardware_interfaces::DriverNames::REAR, mock_rear_driver);
   }
 
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
-  GetMockFrontDriver()
-  {
-    return mock_front_driver_;
-  }
-
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
-  GetMockRearDriver()
-  {
-    return mock_rear_driver_;
-  }
-
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-  GetMockFLMotorDriver()
-  {
-    return mock_fl_motor_driver_;
-  }
-
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-  GetMockFRMotorDriver()
-  {
-    return mock_fr_motor_driver_;
-  }
-
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-  GetMockRLMotorDriver()
-  {
-    return mock_rl_motor_driver_;
-  }
-
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-  GetMockRRMotorDriver()
-  {
-    return mock_rr_motor_driver_;
-  }
-
-private:
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
-    mock_front_driver_;
+    mock_front_driver;
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
-    mock_rear_driver_;
+    mock_rear_driver;
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-    mock_fl_motor_driver_;
+    mock_fl_motor_driver;
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-    mock_fr_motor_driver_;
+    mock_fr_motor_driver;
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-    mock_rl_motor_driver_;
+    mock_rl_motor_driver;
   std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-    mock_rr_motor_driver_;
+    mock_rr_motor_driver;
 };
 
 class TestPantherRobotDriver : public ::testing::Test
@@ -154,22 +117,22 @@ TEST_F(TestPantherRobotDriver, SendSpeedCommands)
   const float rl_v = 0.3;
   const float rr_v = 0.4;
 
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsCANError()).Times(1);
-  EXPECT_CALL(*robot_driver_->GetMockRearDriver(), IsCANError()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).Times(1);
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, IsCANError()).Times(1);
   EXPECT_CALL(
-    *robot_driver_->GetMockFLMotorDriver(),
+    *robot_driver_->mock_fl_motor_driver,
     SendCmdVel(::testing::Eq(static_cast<std::int32_t>(fl_v * kRadPerSecToRbtqCmd))))
     .Times(1);
   EXPECT_CALL(
-    *robot_driver_->GetMockFRMotorDriver(),
+    *robot_driver_->mock_fr_motor_driver,
     SendCmdVel(::testing::Eq(static_cast<std::int32_t>(fr_v * kRadPerSecToRbtqCmd))))
     .Times(1);
   EXPECT_CALL(
-    *robot_driver_->GetMockRLMotorDriver(),
+    *robot_driver_->mock_rl_motor_driver,
     SendCmdVel(::testing::Eq(static_cast<std::int32_t>(rl_v * kRadPerSecToRbtqCmd))))
     .Times(1);
   EXPECT_CALL(
-    *robot_driver_->GetMockRRMotorDriver(),
+    *robot_driver_->mock_rr_motor_driver,
     SendCmdVel(::testing::Eq(static_cast<std::int32_t>(rr_v * kRadPerSecToRbtqCmd))))
     .Times(1);
 
@@ -179,7 +142,7 @@ TEST_F(TestPantherRobotDriver, SendSpeedCommands)
 
 TEST_F(TestPantherRobotDriver, SendSpeedCommandsSendCmdVelError)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFLMotorDriver(), SendCmdVel(::testing::_))
+  EXPECT_CALL(*robot_driver_->mock_fl_motor_driver, SendCmdVel(::testing::_))
     .WillOnce(::testing::Throw(std::runtime_error("")));
 
   EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
@@ -189,7 +152,7 @@ TEST_F(TestPantherRobotDriver, SendSpeedCommandsSendCmdVelError)
 
 TEST_F(TestPantherRobotDriver, SendSpeedCommandsCANError)
 {
-  EXPECT_CALL(*robot_driver_->GetMockFrontDriver(), IsCANError()).WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).WillOnce(::testing::Return(true));
 
   EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { robot_driver_->SendSpeedCommands({0.0, 0.0, 0.0, 0.0}); },

From fefc794d50ff05bde88de21fd6885752a07f3ec7 Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Wed, 11 Sep 2024 14:55:32 +0000
Subject: [PATCH 044/100] apply review suggestions

---
 .../panther_system/robot_driver/robot_driver.hpp   |  2 +-
 .../robot_driver/roboteq_data_converters.hpp       |  4 ++--
 .../robot_driver/roboteq_robot_driver.hpp          |  8 ++++----
 .../robot_driver/lynx_robot_driver.cpp             |  2 +-
 .../robot_driver/roboteq_data_converters.cpp       | 14 +++++++-------
 .../robot_driver/roboteq_robot_driver.cpp          | 10 +++++-----
 .../robot_driver/test_roboteq_data_converters.cpp  |  4 ++--
 7 files changed, 22 insertions(+), 22 deletions(-)

diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
index f7dce3c3..bccd1f64 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
@@ -77,7 +77,7 @@ class RobotDriverInterface
    * @return data feedback
    * @exception std::runtime_error if data with the given name does not exist
    */
-  virtual const RoboteqData & GetData(const std::string & name) = 0;
+  virtual const DriverData & GetData(const std::string & name) = 0;
 
   /**
    * @brief Write speed commands to motors
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp
index 06a3af84..afc071e4 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp
@@ -181,10 +181,10 @@ class RoboteqDriverState
 /**
  * @brief Class that combines all the data that the one Roboteq driver provides
  */
-class RoboteqData
+class DriverData
 {
 public:
-  RoboteqData(const DrivetrainSettings & drivetrain_settings)
+  DriverData(const DrivetrainSettings & drivetrain_settings)
   : channel_1_motor_state_(drivetrain_settings), channel_2_motor_state_(drivetrain_settings)
   {
   }
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
index a43e75cd..3d4e8440 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
@@ -130,7 +130,7 @@ class RoboteqRobotDriver : public RobotDriverInterface
    * @return data feedback
    * @exception std::runtime_error if data with the given name does not exist
    */
-  const RoboteqData & GetData(const std::string & name) override;
+  const DriverData & GetData(const std::string & name) override;
 
 protected:
   /**
@@ -150,16 +150,16 @@ class RoboteqRobotDriver : public RobotDriverInterface
 
 private:
   void SetMotorsStates(
-    RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
+    DriverData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
     const timespec & current_time);
-  void SetDriverState(RoboteqData & data, const DriverState & state, const timespec & current_time);
+  void SetDriverState(DriverData & data, const DriverState & state, const timespec & current_time);
   bool DataTimeout(
     const timespec & current_time, const timespec & data_timestamp,
     const std::chrono::milliseconds & timeout);
 
   bool initialized_ = false;
 
-  std::map<std::string, RoboteqData> data_;
+  std::map<std::string, DriverData> data_;
   RoboteqVelocityCommandConverter roboteq_vel_cmd_converter_;
 
   const std::chrono::milliseconds pdo_motor_states_timeout_ms_;
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
index a86caa5e..54b655af 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
@@ -51,7 +51,7 @@ void LynxRobotDriver::SendSpeedCommands(const std::vector<float> & speeds)
 void LynxRobotDriver::DefineDrivers()
 {
   auto driver = std::make_shared<RoboteqDriver>(
-    canopen_manager_.GetMaster(), canopen_settings_.driver_can_ids.at(DriverNames::FRONT),
+    canopen_manager_.GetMaster(), canopen_settings_.driver_can_ids.at(DriverNames::DEFAULT),
     canopen_settings_.sdo_operation_timeout_ms);
 
   auto left_motor_driver = std::make_shared<RoboteqMotorDriver>(
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp
index 283ebe09..0382edb7 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp
@@ -202,7 +202,7 @@ std::map<std::string, bool> RuntimeError::GetErrorMap() const
   return error_map;
 }
 
-void RoboteqData::SetMotorsStates(
+void DriverData::SetMotorsStates(
   const MotorDriverState & channel_1_state, const MotorDriverState & channel_2_state,
   const bool data_timed_out)
 {
@@ -211,7 +211,7 @@ void RoboteqData::SetMotorsStates(
   motor_states_data_timed_out_ = data_timed_out;
 }
 
-void RoboteqData::SetDriverState(const DriverState & state, const bool data_timed_out)
+void DriverData::SetDriverState(const DriverState & state, const bool data_timed_out)
 {
   driver_state_.SetTemperature(state.mcu_temp);
   driver_state_.SetHeatsinkTemperature(state.heatsink_temp);
@@ -227,7 +227,7 @@ void RoboteqData::SetDriverState(const DriverState & state, const bool data_time
   driver_state_data_timed_out_ = data_timed_out;
 }
 
-const MotorState & RoboteqData::GetMotorState(const std::uint8_t channel) const
+const MotorState & DriverData::GetMotorState(const std::uint8_t channel) const
 {
   if (channel == RoboteqDriver::kChannel1) {
     return channel_1_motor_state_;
@@ -238,7 +238,7 @@ const MotorState & RoboteqData::GetMotorState(const std::uint8_t channel) const
   throw std::runtime_error("Invalid channel number");
 }
 
-const RuntimeError & RoboteqData::GetRuntimeError(const std::uint8_t channel) const
+const RuntimeError & DriverData::GetRuntimeError(const std::uint8_t channel) const
 {
   if (channel == RoboteqDriver::kChannel1) {
     return channel_1_runtime_error_;
@@ -249,7 +249,7 @@ const RuntimeError & RoboteqData::GetRuntimeError(const std::uint8_t channel) co
   throw std::runtime_error("Invalid channel number");
 }
 
-std::string RoboteqData::GetFlagErrorLog() const
+std::string DriverData::GetFlagErrorLog() const
 {
   return "Fault flags: " + fault_flags_.GetErrorLog() +
          "Script flags: " + script_flags_.GetErrorLog() +
@@ -257,7 +257,7 @@ std::string RoboteqData::GetFlagErrorLog() const
          "Channel 2 motor runtime flags: " + channel_2_runtime_error_.GetErrorLog();
 }
 
-std::map<std::string, bool> RoboteqData::GetFlagErrorMap() const
+std::map<std::string, bool> DriverData::GetFlagErrorMap() const
 {
   std::map<std::string, bool> flag_error_map;
 
@@ -276,7 +276,7 @@ std::map<std::string, bool> RoboteqData::GetFlagErrorMap() const
   return flag_error_map;
 }
 
-std::map<std::string, bool> RoboteqData::GetErrorMap() const
+std::map<std::string, bool> DriverData::GetErrorMap() const
 {
   std::map<std::string, bool> error_map;
 
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
index a4ccad8b..81026556 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
@@ -55,11 +55,11 @@ void RoboteqRobotDriver::Initialize()
     canopen_manager_.Initialize();
     DefineDrivers();
     for (auto & [name, driver] : drivers_) {
-      data_.emplace(name, RoboteqData(drivetrain_settings_));
+      data_.emplace(name, DriverData(drivetrain_settings_));
       driver->Boot();
     }
   } catch (const std::runtime_error & e) {
-    throw e;
+    throw std::runtime_error("Failed to initialize robot driver: " + std::string(e.what()));
   }
 
   initialized_ = true;
@@ -157,7 +157,7 @@ void RoboteqRobotDriver::UpdateDriversState()
   }
 }
 
-const RoboteqData & RoboteqRobotDriver::GetData(const std::string & name)
+const DriverData & RoboteqRobotDriver::GetData(const std::string & name)
 {
   if (data_.find(name) == data_.end()) {
     throw std::runtime_error("Data with name '" + name + "' does not exist.");
@@ -191,7 +191,7 @@ void RoboteqRobotDriver::TurnOffEStop()
 }
 
 void RoboteqRobotDriver::SetMotorsStates(
-  RoboteqData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
+  DriverData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
   const timespec & current_time)
 {
   const bool data_timed_out =
@@ -205,7 +205,7 @@ void RoboteqRobotDriver::SetMotorsStates(
 }
 
 void RoboteqRobotDriver::SetDriverState(
-  RoboteqData & data, const DriverState & state, const timespec & current_time)
+  DriverData & data, const DriverState & state, const timespec & current_time)
 {
   const bool data_timed_out =
     DataTimeout(current_time, state.flags_current_timestamp, pdo_driver_state_timeout_ms_) ||
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_data_converters.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_data_converters.cpp
index 8780282e..1bba2723 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_data_converters.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_data_converters.cpp
@@ -231,14 +231,14 @@ TEST(TestRoboteqDataConverters, DriverState)
   EXPECT_FLOAT_EQ(driver_state.GetCurrent(), 3.5);
 }
 
-TEST(TestRoboteqDataConverters, RoboteqData)
+TEST(TestRoboteqDataConverters, DriverData)
 {
   using panther_hardware_interfaces::RoboteqDriver;
   using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
   using panther_hardware_interfaces_test::kRbtqPosFbToRad;
   using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
 
-  panther_hardware_interfaces::RoboteqData roboteq_data(
+  panther_hardware_interfaces::DriverData roboteq_data(
     panther_hardware_interfaces_test::kDrivetrainSettings);
 
   ASSERT_FALSE(roboteq_data.IsError());

From 25102d18241c1f65be8fc850151baf8e89c022e0 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Tue, 17 Sep 2024 12:27:21 +0200
Subject: [PATCH 045/100] Working loading arguments from robot_configuration
 YAML (#413)

---
 panther_bringup/launch/bringup.launch.py      |   3 +-
 .../launch/simulate_multiple_robots.launch.py | 120 -------
 .../launch/simulate_robot.launch.py           |  29 +-
 panther_gazebo/launch/simulation.launch.py    |  34 +-
 panther_gazebo/launch/spawn_robot.launch.py   |  23 +-
 panther_utils/panther_utils/arguments.py      | 305 ++++++++++--------
 panther_utils/panther_utils/messages.py       |  42 ++-
 7 files changed, 243 insertions(+), 313 deletions(-)
 delete mode 100644 panther_gazebo/launch/simulate_multiple_robots.launch.py

diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py
index ebb5bc1d..716c9c9c 100644
--- a/panther_bringup/launch/bringup.launch.py
+++ b/panther_bringup/launch/bringup.launch.py
@@ -51,9 +51,10 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
+    robot_model = EnvironmentVariable("ROBOT_MODEL", default_value="panther")
     serial_no = EnvironmentVariable(name="PANTHER_SERIAL_NO", default_value="----")
     panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0")
-    welcome_info = welcome_msg(serial_no, panther_version)
+    welcome_info = welcome_msg(robot_model, serial_no, panther_version)
 
     controller_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
diff --git a/panther_gazebo/launch/simulate_multiple_robots.launch.py b/panther_gazebo/launch/simulate_multiple_robots.launch.py
deleted file mode 100644
index 6c8c1bc7..00000000
--- a/panther_gazebo/launch/simulate_multiple_robots.launch.py
+++ /dev/null
@@ -1,120 +0,0 @@
-#!/usr/bin/env python3
-
-# 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.
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction
-from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import (
-    LaunchConfiguration,
-    PathJoinSubstitution,
-    PythonExpression,
-)
-from launch_ros.actions import Node, SetUseSimTime
-from launch_ros.substitutions import FindPackageShare
-from panther_utils.arguments import load_yaml_file, normalize_robot_configuration
-
-
-def generate_launch_description():
-    add_world_transform = LaunchConfiguration("add_world_transform")
-    declare_add_world_transform_arg = DeclareLaunchArgument(
-        "add_world_transform",
-        default_value="False",
-        description=(
-            "Adds a world frame that connects the tf trees of individual robots (useful when running"
-            " multiple robots)."
-        ),
-        choices=["True", "true", "False", "false"],
-    )
-
-    path = PathJoinSubstitution(
-        [FindPackageShare("panther_gazebo"), "config", "configuration.yaml"]
-    )
-    yaml_data = load_yaml_file(path)
-    yaml_data = normalize_robot_configuration(yaml_data)
-
-    simulate_robot = []
-    idx = 0
-    for namespace, robot_config in yaml_data.items():
-        robot_model = robot_config.get("robot_model", "panther")
-        x, y, z = robot_config.get("init_pose", [0.0, 0.0, 0.0])
-        roll, pitch, yaw = robot_config.get("init_rotation", [0.0, 0.0, 0.0])
-        x, y, z, roll, pitch, yaw = map(str, [x, y, z, roll, pitch, yaw])
-        configuration_data = robot_config.get("configuration", {})
-        wheel_type = configuration_data.get("wheel_type", "")
-
-        # Prefer declaration over configuration:
-        namespace = LaunchConfiguration("namespace", default=namespace)
-        robot_model = LaunchConfiguration("robot_model", default=robot_model)
-        wheel_type = LaunchConfiguration("wheel_type", default=wheel_type)
-        x = LaunchConfiguration("x", default=x)
-        y = LaunchConfiguration("y", default=y)
-        z = LaunchConfiguration("z", default=z)
-        roll = LaunchConfiguration("roll", default=roll)
-        pitch = LaunchConfiguration("pitch", default=pitch)
-        yaw = LaunchConfiguration("yaw", default=yaw)
-
-        spawn_robot_launch = IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [FindPackageShare("panther_gazebo"), "launch", "simulate_robot.launch.py"]
-                )
-            ),
-            launch_arguments={
-                "namespace": namespace,
-                "robot_model": robot_model,
-                "wheel_type": wheel_type,
-                "x": x,
-                "y": y,
-                "z": z,
-                "roll": roll,
-                "pitch": pitch,
-                "yaw": yaw,
-            }.items(),
-        )
-
-        child_tf = PythonExpression(
-            ["'", namespace, "' + '/odom' if '", namespace, "' else 'odom'"]
-        )
-
-        world_transform = Node(
-            package="tf2_ros",
-            executable="static_transform_publisher",
-            name="static_tf_publisher",
-            arguments=[x, y, z, roll, pitch, yaw, "world", child_tf],
-            namespace=namespace,
-            emulate_tty=True,
-            condition=IfCondition(add_world_transform),
-        )
-
-        # Add small delay to prevent namespace overwriting
-        group = TimerAction(
-            period=5.0 * idx,
-            actions=[
-                spawn_robot_launch,
-                world_transform,
-            ],
-        )
-        idx += 1
-        simulate_robot.append(group)
-
-    return LaunchDescription(
-        [
-            declare_add_world_transform_arg,
-            SetUseSimTime(True),
-            *simulate_robot,
-        ]
-    )
diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py
index 573df926..4c271a8e 100644
--- a/panther_gazebo/launch/simulate_robot.launch.py
+++ b/panther_gazebo/launch/simulate_robot.launch.py
@@ -26,11 +26,18 @@
 from launch_ros.actions import Node, SetUseSimTime
 from launch_ros.substitutions import FindPackageShare
 from nav2_common.launch import ReplaceString
-from panther_utils.arguments import declare_robot_args
+from panther_utils.arguments import DeclareRobotArgs
 
 
 def generate_launch_description():
 
+    components_config_path = LaunchConfiguration("components_config_path")
+    gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path")
+    namespace = LaunchConfiguration("namespace")
+    robot_configuration = LaunchConfiguration("robot_configuration")
+    robot_model = LaunchConfiguration("robot_model")
+    use_ekf = LaunchConfiguration("use_ekf")
+
     declare_battery_config_path_arg = DeclareLaunchArgument(
         "battery_config_path",
         description=(
@@ -42,7 +49,6 @@ def generate_launch_description():
         ),
     )
 
-    components_config_path = LaunchConfiguration("components_config_path")
     declare_components_config_path_arg = DeclareLaunchArgument(
         "components_config_path",
         default_value=PathJoinSubstitution(
@@ -56,7 +62,6 @@ def generate_launch_description():
         ),
     )
 
-    gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path")
     declare_gz_bridge_config_path_arg = DeclareLaunchArgument(
         "gz_bridge_config_path",
         default_value=PathJoinSubstitution(
@@ -65,10 +70,14 @@ def generate_launch_description():
         description="Path to the parameter_bridge configuration file.",
     )
 
-    namespace = LaunchConfiguration("namespace")
-    robot_model = LaunchConfiguration("robot_model")
+    declare_robot_configuration_arg = DeclareLaunchArgument(
+        "robot_configuration",
+        default_value=PathJoinSubstitution(
+            [FindPackageShare("panther_gazebo"), "config", "configuration.yaml"]
+        ),
+        description="Path to robot configuration YAML file.",
+    )
 
-    use_ekf = LaunchConfiguration("use_ekf")
     declare_use_ekf_arg = DeclareLaunchArgument(
         "use_ekf",
         default_value="True",
@@ -182,14 +191,10 @@ def generate_launch_description():
         emulate_tty=True,
     )
 
-    path = PathJoinSubstitution(
-        [FindPackageShare("panther_gazebo"), "config", "configuration.yaml"]
-    )
-    list_of_robot_args = declare_robot_args(path)
-
     return LaunchDescription(
         [
-            *list_of_robot_args,
+            declare_robot_configuration_arg,
+            DeclareRobotArgs(robot_configuration),
             declare_battery_config_path_arg,
             declare_components_config_path_arg,
             declare_gz_bridge_config_path_arg,
diff --git a/panther_gazebo/launch/simulation.launch.py b/panther_gazebo/launch/simulation.launch.py
index 2093b881..548100e0 100644
--- a/panther_gazebo/launch/simulation.launch.py
+++ b/panther_gazebo/launch/simulation.launch.py
@@ -17,20 +17,19 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import (
-    EnvironmentVariable,
-    LaunchConfiguration,
-    PathJoinSubstitution,
-)
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import SetUseSimTime
 from launch_ros.substitutions import FindPackageShare
 from nav2_common.launch import ReplaceString
-from panther_utils.arguments import load_yaml_file, normalize_robot_configuration
+from panther_utils.arguments import DeclareRobotArgs
 
 
 def generate_launch_description():
 
     gz_gui = LaunchConfiguration("gz_gui")
+    namespace = LaunchConfiguration("namespace")
+    robot_configuration = LaunchConfiguration("robot_configuration")
+
     declare_gz_gui = DeclareLaunchArgument(
         "gz_gui",
         default_value=PathJoinSubstitution(
@@ -39,18 +38,12 @@ def generate_launch_description():
         description="Run simulation with specific GUI layout.",
     )
 
-    path = PathJoinSubstitution(
-        [FindPackageShare("panther_gazebo"), "config", "configuration.yaml"]
-    )
-    yaml_data = load_yaml_file(path)
-    yaml_data = normalize_robot_configuration(yaml_data)
-    ns_from_config = list(yaml_data.keys())[0]
-
-    namespace = LaunchConfiguration("namespace")
-    declare_namespace_arg = DeclareLaunchArgument(
-        "namespace",
-        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=ns_from_config),
-        description="Add namespace to all launched nodes.",
+    declare_robot_configuration_arg = DeclareLaunchArgument(
+        "robot_configuration",
+        default_value=PathJoinSubstitution(
+            [FindPackageShare("panther_gazebo"), "config", "configuration.yaml"]
+        ),
+        description="Path to robot configuration YAML file.",
     )
 
     namespaced_gz_gui = ReplaceString(
@@ -73,7 +66,7 @@ def generate_launch_description():
                 [
                     FindPackageShare("panther_gazebo"),
                     "launch",
-                    "simulate_multiple_robots.launch.py",
+                    "simulate_robot.launch.py",
                 ]
             )
         ),
@@ -82,7 +75,8 @@ def generate_launch_description():
     return LaunchDescription(
         [
             declare_gz_gui,
-            declare_namespace_arg,
+            declare_robot_configuration_arg,
+            DeclareRobotArgs(robot_configuration),
             # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo)
             SetUseSimTime(True),
             gz_sim,
diff --git a/panther_gazebo/launch/spawn_robot.launch.py b/panther_gazebo/launch/spawn_robot.launch.py
index d7bcc378..5a54c369 100644
--- a/panther_gazebo/launch/spawn_robot.launch.py
+++ b/panther_gazebo/launch/spawn_robot.launch.py
@@ -15,7 +15,7 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
     LaunchConfiguration,
@@ -24,13 +24,14 @@
 )
 from launch_ros.actions import Node, SetUseSimTime
 from launch_ros.substitutions import FindPackageShare
-from panther_utils.arguments import declare_robot_args
+from panther_utils.arguments import DeclareRobotArgs
 from panther_utils.messages import welcome_msg
 
 
 def generate_launch_description():
 
     namespace = LaunchConfiguration("namespace")
+    robot_configuration = LaunchConfiguration("robot_configuration")
     robot_model = LaunchConfiguration("robot_model")
     x = LaunchConfiguration("x")
     y = LaunchConfiguration("y")
@@ -39,11 +40,19 @@ def generate_launch_description():
     pitch = LaunchConfiguration("pitch")
     yaw = LaunchConfiguration("yaw")
 
+    declare_robot_configuration_arg = DeclareLaunchArgument(
+        "robot_configuration",
+        default_value=PathJoinSubstitution(
+            [FindPackageShare("panther_gazebo"), "config", "configuration.yaml"]
+        ),
+        description="Path to robot configuration YAML file.",
+    )
+
     log_stats = {
         "Robot namespace": namespace,
         "Initial pose": ["(", x, ", ", y, ", ", z, ", ", roll, ", ", pitch, ", ", yaw, ")"],
     }
-    welcome_info = welcome_msg("---", "simulation", log_stats)
+    welcome_info = welcome_msg(robot_model, "----", "simulation", log_stats)
 
     urdf_packages = PythonExpression(["'", robot_model, "_description'"])
     add_wheel_joints = LaunchConfiguration("add_wheel_joints", default="True")
@@ -87,14 +96,10 @@ def generate_launch_description():
         emulate_tty=True,
     )
 
-    path = PathJoinSubstitution(
-        [FindPackageShare("panther_gazebo"), "config", "configuration.yaml"]
-    )
-    list_of_robot_args = declare_robot_args(path)
-
     return LaunchDescription(
         [
-            *list_of_robot_args,
+            declare_robot_configuration_arg,
+            DeclareRobotArgs(robot_configuration),
             SetUseSimTime(True),
             welcome_info,
             load_urdf,
diff --git a/panther_utils/panther_utils/arguments.py b/panther_utils/panther_utils/arguments.py
index 87443767..1cea5d56 100644
--- a/panther_utils/panther_utils/arguments.py
+++ b/panther_utils/panther_utils/arguments.py
@@ -15,12 +15,15 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from typing import Tuple
+from typing import List, Text, Tuple
 
 import yaml
-from launch import LaunchContext, Substitution
+from launch.action import Action
 from launch.actions import DeclareLaunchArgument
+from launch.frontend import Entity, Parser
+from launch.launch_context import LaunchContext
 from launch.substitutions import EnvironmentVariable, LaunchConfiguration
+from launch.utilities import normalize_to_list_of_substitutions, perform_substitutions
 
 # Define valid configurations for each robot model
 VALID_CONFIGURATIONS = {
@@ -29,141 +32,165 @@
 }
 
 
-def resolve_path(path: str | Substitution) -> str:
-    """Resolve a path that might be substitutions."""
-    if isinstance(path, Substitution):
-        return path.perform(LaunchContext())
-    return path
-
-
-def load_yaml_file(path: str | Substitution) -> dict:
-    """Load YAML file and return its contents."""
-    path = resolve_path(path)
-    try:
-        with open(path, "r") as file:
-            return yaml.safe_load(file)
-    except yaml.YAMLError as exc:
-        raise ValueError(f"Error reading YAML file: {exc}") from exc
-
-
-def validate_robot_configuration(yaml_data: dict) -> None:
-    """Validate the robot model and wheel type configuration."""
-    robot_model = yaml_data.get("robot_model", "")
-    configuration_data = yaml_data.get("configuration", {})
-    wheel_type = configuration_data.get("wheel_type", "")
-
-    if robot_model not in VALID_CONFIGURATIONS:
-        raise ValueError(
-            f"Invalid robot model '{robot_model}'. "
-            f"Valid models are: {', '.join(VALID_CONFIGURATIONS.keys())}"
-        )
-
-    valid_wheel_types = VALID_CONFIGURATIONS[robot_model]["wheel_type"]
-    if wheel_type not in valid_wheel_types:
-        raise ValueError(
-            f"Invalid wheel type '{wheel_type}' for {robot_model}. "
-            f"Valid wheel types are: {', '.join(valid_wheel_types)}"
-        )
-
-
-def normalize_robot_configuration(yaml_data: dict) -> dict:
-    """Normalizes the YAML dictionary structure to a flat to nested format and validates the structure."""
-
-    obligatory_keys = {"configuration"}
-
-    # Checking and normalize flat structure
-    if obligatory_keys.issubset(yaml_data.keys()):
-        validate_robot_configuration(yaml_data)
-        return {"": yaml_data}
-
-    # Checking nested structure
-    if all(isinstance(value, dict) for value in yaml_data.values()):
-        nested_data = {}
-        for namespace, robot_config in yaml_data.items():
-            if not obligatory_keys.issubset(robot_config.keys()):
-                raise ValueError(
-                    f"Invalid nested YAML structure at '{namespace}': Missing required keys."
-                )
-            validate_robot_configuration(robot_config)
-            nested_data[namespace] = robot_config
-        return nested_data
-
-    raise ValueError("Invalid YAML structure: The data does not match expected formats.")
-
-
-def extract_single_robot_configuration(yaml_data: dict, idx: int = 0) -> Tuple[str, dict]:
-    """Extracts the namespace and configuration based on the provided index from a YAML dictionary."""
-    keys = list(yaml_data.keys())
-    namespace = keys[idx]
-    configuration = yaml_data[namespace]
-    return namespace, configuration
-
-
-def create_launch_arguments(namespace: str, robot_config: dict) -> list[DeclareLaunchArgument]:
-    """Declare launch arguments based on the YAML configuration files."""
-    robot_model = robot_config.get("robot_model", "panther")
-    x, y, z = robot_config.get("init_pose", [0.0, 0.0, 0.0])
-    roll, pitch, yaw = robot_config.get("init_rotation", [0.0, 0.0, 0.0])
-    x, y, z, roll, pitch, yaw = map(str, [x, y, z, roll, pitch, yaw])
-    configuration_data = robot_config.get("configuration", {})
-    wheel_type = configuration_data.get("wheel_type", "")
-
-    # Prefer declaration over configuration:
-    namespace = LaunchConfiguration("namespace", default=namespace)
-    robot_model = LaunchConfiguration("robot_model", default=robot_model)
-    wheel_type = LaunchConfiguration("wheel_type", default=wheel_type)
-    x = LaunchConfiguration("x", default=x)
-    y = LaunchConfiguration("y", default=y)
-    z = LaunchConfiguration("z", default=z)
-    roll = LaunchConfiguration("roll", default=roll)
-    pitch = LaunchConfiguration("pitch", default=pitch)
-    yaw = LaunchConfiguration("yaw", default=yaw)
-
-    return [
-        DeclareLaunchArgument(
-            "namespace",
-            default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=namespace),
-            description="Add namespace to all launched nodes.",
-        ),
-        DeclareLaunchArgument(
-            "robot_model",
-            default_value=EnvironmentVariable("ROBOT_MODEL", default_value=robot_model),
-            description="Specify robot model type.",
-        ),
-        DeclareLaunchArgument(
-            "wheel_type",
-            default_value=wheel_type,
-            description=(
-                "Specify the wheel type. If the selected wheel type is not 'custom', "
-                "the 'wheel_config_path' and 'controller_config_path' arguments will be "
-                "automatically adjusted and can be omitted."
-            ),
-        ),
-        DeclareLaunchArgument(
-            "x", default_value=x, description="Initial robot position in the global 'x' axis."
-        ),
-        DeclareLaunchArgument(
-            "y", default_value=y, description="Initial robot position in the global 'y' axis."
-        ),
-        DeclareLaunchArgument(
-            "z", default_value=z, description="Initial robot position in the global 'z' axis."
-        ),
-        DeclareLaunchArgument(
-            "roll", default_value=roll, description="Initial robot 'roll' orientation."
-        ),
-        DeclareLaunchArgument(
-            "pitch", default_value=pitch, description="Initial robot 'pitch' orientation."
-        ),
-        DeclareLaunchArgument(
-            "yaw", default_value=yaw, description="Initial robot 'yaw' orientation."
-        ),
-    ]
-
-
-def declare_robot_args(path: str | Substitution) -> list[DeclareLaunchArgument]:
+class DeclareRobotArgs(Action):
     """Retrieves and validate the robot configuration from the YAML data."""
-    yaml_data = load_yaml_file(path)
-    yaml_data = normalize_robot_configuration(yaml_data)
-    namespace, robot_config = extract_single_robot_configuration(yaml_data)
-    list_of_args = create_launch_arguments(namespace, robot_config)
-    return list_of_args
+
+    def __init__(self, path: LaunchConfiguration, **kwargs) -> None:
+        """Create a DeclareRobotArgs action."""
+        super().__init__(**kwargs)
+        self.__path = normalize_to_list_of_substitutions(path)
+
+    @classmethod
+    def parse(cls, entity: Entity, parser: "Parser"):
+        """Parse `arg` tag."""
+        _, kwargs = super().parse(entity, parser)
+        kwargs["name"] = parser.escape_characters(entity.get_attr("name"))
+        default_value = entity.get_attr("default", optional=True)
+        if default_value is not None:
+            kwargs["default_value"] = parser.parse_substitution(default_value)
+        description = entity.get_attr("description", optional=True)
+        if description is not None:
+            kwargs["description"] = parser.escape_characters(description)
+        choices = entity.get_attr("choice", data_type=List[Entity], optional=True)
+        if choices is not None:
+            kwargs["choices"] = [
+                parser.escape_characters(choice.get_attr("value")) for choice in choices
+            ]
+        return cls, kwargs
+
+    @property
+    def path(self) -> Text:
+        """Getter for self.__path."""
+        return self.__path
+
+    def execute(self, context: LaunchContext):
+        """Execute the action."""
+        path = perform_substitutions(context, self.path)
+        yaml_data = self.load_yaml_file(path)
+        yaml_data = self.normalize_robot_configuration(yaml_data)
+        namespace, robot_config = self.extract_single_robot_configuration(yaml_data)
+        list_of_args = self.create_launch_arguments(namespace, robot_config)
+        for arg in list_of_args:
+            arg.execute(context)
+
+    def load_yaml_file(self, path: str) -> dict:
+        """Load YAML file and return its contents."""
+        try:
+            with open(path, "r") as file:
+                return yaml.safe_load(file)
+        except yaml.YAMLError as exc:
+            raise ValueError(f"Error reading YAML file: {exc}") from exc
+
+    def validate_robot_configuration(self, yaml_data: dict) -> None:
+        """Validate the robot model and wheel type configuration."""
+        robot_model = yaml_data.get("robot_model", "")
+        configuration_data = yaml_data.get("configuration", {})
+        wheel_type = configuration_data.get("wheel_type", "")
+
+        if robot_model not in VALID_CONFIGURATIONS:
+            raise ValueError(
+                f"Invalid robot model '{robot_model}'. "
+                f"Valid models are: {', '.join(VALID_CONFIGURATIONS.keys())}"
+            )
+
+        valid_wheel_types = VALID_CONFIGURATIONS[robot_model]["wheel_type"]
+        if wheel_type not in valid_wheel_types:
+            raise ValueError(
+                f"Invalid wheel type '{wheel_type}' for {robot_model}. "
+                f"Valid wheel types are: {', '.join(valid_wheel_types)}"
+            )
+
+    def normalize_robot_configuration(self, yaml_data: dict) -> dict:
+        """Normalizes the YAML dictionary structure to a flat to nested format and validates the structure."""
+
+        obligatory_keys = {"configuration"}
+
+        # Checking and normalize flat structure
+        if obligatory_keys.issubset(yaml_data.keys()):
+            self.validate_robot_configuration(yaml_data)
+            return {"": yaml_data}
+
+        # Checking nested structure
+        if all(isinstance(value, dict) for value in yaml_data.values()):
+            nested_data = {}
+            for namespace, robot_config in yaml_data.items():
+                if not obligatory_keys.issubset(robot_config.keys()):
+                    raise ValueError(
+                        f"Invalid nested YAML structure at '{namespace}': Missing required keys."
+                    )
+                self.validate_robot_configuration(robot_config)
+                nested_data[namespace] = robot_config
+            return nested_data
+
+        raise ValueError("Invalid YAML structure: The data does not match expected formats.")
+
+    def extract_single_robot_configuration(
+        self, yaml_data: dict, idx: int = 0
+    ) -> Tuple[str, dict]:
+        """Extracts the namespace and configuration based on the provided index from a YAML dictionary."""
+        keys = list(yaml_data.keys())
+        namespace = keys[idx]
+        configuration = yaml_data[namespace]
+        return namespace, configuration
+
+    def create_launch_arguments(
+        self, namespace: str, robot_config: dict
+    ) -> list[DeclareLaunchArgument]:
+        """Declare launch arguments based on the YAML configuration files."""
+        robot_model = robot_config.get("robot_model", "panther")
+        x, y, z = robot_config.get("init_pose", [0.0, 0.0, 0.0])
+        roll, pitch, yaw = robot_config.get("init_rotation", [0.0, 0.0, 0.0])
+        x, y, z, roll, pitch, yaw = map(str, [x, y, z, roll, pitch, yaw])
+        configuration_data = robot_config.get("configuration", {})
+        wheel_type = configuration_data.get("wheel_type", "")
+
+        # Prefer declaration over configuration:
+        namespace = LaunchConfiguration("namespace", default=namespace)
+        robot_model = LaunchConfiguration("robot_model", default=robot_model)
+        wheel_type = LaunchConfiguration("wheel_type", default=wheel_type)
+        x = LaunchConfiguration("x", default=x)
+        y = LaunchConfiguration("y", default=y)
+        z = LaunchConfiguration("z", default=z)
+        roll = LaunchConfiguration("roll", default=roll)
+        pitch = LaunchConfiguration("pitch", default=pitch)
+        yaw = LaunchConfiguration("yaw", default=yaw)
+
+        return [
+            DeclareLaunchArgument(
+                "namespace",
+                default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=namespace),
+                description="Add namespace to all launched nodes.",
+            ),
+            DeclareLaunchArgument(
+                "robot_model",
+                default_value=EnvironmentVariable("ROBOT_MODEL", default_value=robot_model),
+                description="Specify robot model type.",
+            ),
+            DeclareLaunchArgument(
+                "wheel_type",
+                default_value=wheel_type,
+                description=(
+                    "Specify the wheel type. If the selected wheel type is not 'custom', "
+                    "the 'wheel_config_path' and 'controller_config_path' arguments will be "
+                    "automatically adjusted and can be omitted."
+                ),
+            ),
+            DeclareLaunchArgument(
+                "x", default_value=x, description="Initial robot position in the global 'x' axis."
+            ),
+            DeclareLaunchArgument(
+                "y", default_value=y, description="Initial robot position in the global 'y' axis."
+            ),
+            DeclareLaunchArgument(
+                "z", default_value=z, description="Initial robot position in the global 'z' axis."
+            ),
+            DeclareLaunchArgument(
+                "roll", default_value=roll, description="Initial robot 'roll' orientation."
+            ),
+            DeclareLaunchArgument(
+                "pitch", default_value=pitch, description="Initial robot 'pitch' orientation."
+            ),
+            DeclareLaunchArgument(
+                "yaw", default_value=yaw, description="Initial robot 'yaw' orientation."
+            ),
+        ]
diff --git a/panther_utils/panther_utils/messages.py b/panther_utils/panther_utils/messages.py
index 4ada1f51..33f34e22 100644
--- a/panther_utils/panther_utils/messages.py
+++ b/panther_utils/panther_utils/messages.py
@@ -20,10 +20,33 @@
 import click
 from launch.actions import LogInfo
 from launch.some_substitutions_type import SomeSubstitutionsType
-from launch.substitutions import Command
+from launch.substitutions import Command, PythonExpression
+
+LYNX_ASCII = r"""
+     _
+    | |   _   _ _ __ __  __
+    | |  | | | | '_ \\ \/ /
+    | |__| |_| | | | |>  <
+    |_____\__, |_| |_/_/\_\
+          |___/
+
+    """  # noqa: W605
+
+PANTHER_ASCII = r"""
+     ____             _   _
+    |  _ \ __ _ _ __ | |_| |__   ___ _ __
+    | |_) / _` | '_ \| __| '_ \ / _ \ '__|
+    |  __/ (_| | | | | |_| | | |  __/ |
+    |_|   \__,_|_| |_|\__|_| |_|\___|_|
+
+    """  # noqa: W605
+
+LYNX_TEXT = click.style(textwrap.dedent(LYNX_ASCII), bold=True)
+PANTHER_TEXT = click.style(textwrap.dedent(PANTHER_ASCII), bold=True)
 
 
 def flatten(lst):
+    """Flatten a nested list into a single list."""
     if isinstance(lst, list):
         flat_list = []
         for element in lst:
@@ -34,22 +57,17 @@ def flatten(lst):
 
 
 def welcome_msg(
+    robot_model: SomeSubstitutionsType,
     serial_number: SomeSubstitutionsType,
     robot_hw_version: SomeSubstitutionsType,
     additional_stats: Dict = {},
 ):
+    """Generate a welcome message with robot information and stats."""
     pkg_version = Command(command="ros2 pkg xml -t version panther")
 
-    PANTHER_TEXT = """
-     ____             _   _
-    |  _ \ __ _ _ __ | |_| |__   ___ _ __
-    | |_) / _` | '_ \| __| '_ \ / _ \ '__|
-    |  __/ (_| | | | | |_| | | |  __/ |
-    |_|   \__,_|_| |_|\__|_| |_|\___|_|
-
-    """  # noqa: W605
-    pth_txt = textwrap.dedent(PANTHER_TEXT)
-    pth_txt = click.style(pth_txt, bold=True)
+    robot_model_expr = PythonExpression(
+        [f"r'''{LYNX_TEXT}''' if '", robot_model, f"' == 'lynx' else r'''{PANTHER_TEXT}'''"]
+    )
 
     stats_to_show = {
         "Serial Number": serial_number,
@@ -68,6 +86,6 @@ def welcome_msg(
     ]
     stats_msg = flatten(nested_list_of_stats)
 
-    stats_msg.insert(0, pth_txt)
+    stats_msg.insert(0, robot_model_expr)
 
     return LogInfo(msg=stats_msg)

From 4904c3f2a8bd5511abd09694297e34b98d59684d Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Tue, 17 Sep 2024 13:52:36 +0200
Subject: [PATCH 046/100] Update panther_gazebo/config/configuration.yaml

---
 panther_gazebo/config/configuration.yaml | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/panther_gazebo/config/configuration.yaml b/panther_gazebo/config/configuration.yaml
index c4ed5b5b..7f817003 100644
--- a/panther_gazebo/config/configuration.yaml
+++ b/panther_gazebo/config/configuration.yaml
@@ -1,7 +1,5 @@
 # The minimal configuration file with the default values.
-# namespace is optional - without this element namespace will not be provided.
-
-# namespace:
+# namespace: (optional - without this element namespace will not be provided)
 #   robot_model: panther
 #   init_pose: [0.0, 0.0, 0.0]
 #   init_rotation: [0.0, 0.0, 0.0]

From c4ec3c9cf01021e20e918279875384b79ed68beb Mon Sep 17 00:00:00 2001
From: Dawid Kmak <73443304+KmakD@users.noreply.github.com>
Date: Tue, 17 Sep 2024 15:33:37 +0200
Subject: [PATCH 047/100] Ros2 lynx system (#412)

* update system ros interface

* update system e-stop

* add generic ugv_system

* fix ugv_system

* fix robot driver initialization

* add lynx system

* Add panther system implementation

* add get speed commands tests

* add system ros interface tests

* update docs

* review suggestions

* update README.md
---
 lynx_description/urdf/lynx_macro.urdf.xacro   |   5 +-
 panther_hardware_interfaces/CMakeLists.txt    | 146 +++-
 panther_hardware_interfaces/README.md         |  31 +-
 .../panther_system/lynx_system.hpp            |  60 ++
 .../panther_system/panther_system.hpp         | 129 +--
 .../panther_system/panther_system_e_stop.hpp  | 191 -----
 .../robot_driver/lynx_robot_driver.hpp        |   2 +-
 .../robot_driver/robot_driver.hpp             |   7 +
 .../robot_driver/roboteq_robot_driver.hpp     |   3 +
 .../panther_system/system_e_stop.hpp          | 120 +++
 ...interface.hpp => system_ros_interface.hpp} |  76 +-
 .../panther_system/ugv_system.hpp             | 168 ++++
 .../panther_hardware_interfaces.xml           |   8 +
 .../src/panther_system/lynx_system.cpp        | 176 ++++
 .../src/panther_system/panther_system.cpp     | 670 ++-------------
 .../robot_driver/roboteq_robot_driver.cpp     |  39 +-
 ...er_system_e_stop.cpp => system_e_stop.cpp} |  65 +-
 ...interface.cpp => system_ros_interface.cpp} | 119 ++-
 .../src/panther_system/ugv_system.cpp         | 550 +++++++++++++
 .../panther_system/test_panther_system.cpp    | 777 ------------------
 .../test_panther_system_ros_interface.cpp     | 285 -------
 .../test_roboteq_robot_driver.cpp             |  55 +-
 .../unit/panther_system/test_lynx_system.cpp  | 311 +++++++
 .../panther_system/test_panther_system.cpp    | 375 +++++++++
 .../test_system_ros_interface.cpp             | 207 +++++
 .../unit/panther_system/test_ugv_system.cpp   | 378 +++++++++
 .../test/utils/mock_driver.hpp                |  13 +
 .../test/utils/system_test_utils.hpp          | 174 ++++
 28 files changed, 2939 insertions(+), 2201 deletions(-)
 create mode 100644 panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/lynx_system.hpp
 delete mode 100644 panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp
 create mode 100644 panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_e_stop.hpp
 rename panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/{panther_system_ros_interface.hpp => system_ros_interface.hpp} (85%)
 create mode 100644 panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/ugv_system.hpp
 create mode 100644 panther_hardware_interfaces/src/panther_system/lynx_system.cpp
 rename panther_hardware_interfaces/src/panther_system/{panther_system_e_stop.cpp => system_e_stop.cpp} (57%)
 rename panther_hardware_interfaces/src/panther_system/{panther_system_ros_interface.cpp => system_ros_interface.cpp} (61%)
 create mode 100644 panther_hardware_interfaces/src/panther_system/ugv_system.cpp
 delete mode 100644 panther_hardware_interfaces/test/panther_system/test_panther_system.cpp
 delete mode 100644 panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
 create mode 100644 panther_hardware_interfaces/test/unit/panther_system/test_lynx_system.cpp
 create mode 100644 panther_hardware_interfaces/test/unit/panther_system/test_panther_system.cpp
 create mode 100644 panther_hardware_interfaces/test/unit/panther_system/test_system_ros_interface.cpp
 create mode 100644 panther_hardware_interfaces/test/unit/panther_system/test_ugv_system.cpp
 create mode 100644 panther_hardware_interfaces/test/utils/system_test_utils.hpp

diff --git a/lynx_description/urdf/lynx_macro.urdf.xacro b/lynx_description/urdf/lynx_macro.urdf.xacro
index 94b691ae..4f1d9119 100644
--- a/lynx_description/urdf/lynx_macro.urdf.xacro
+++ b/lynx_description/urdf/lynx_macro.urdf.xacro
@@ -61,7 +61,7 @@
         </xacro:if>
 
         <xacro:unless value="$(arg use_sim)">
-          <plugin>panther_hardware_interfaces/PantherSystem</plugin>
+          <plugin>panther_hardware_interfaces/LynxSystem</plugin>
 
           <param name="encoder_resolution">1600</param>
 
@@ -76,8 +76,7 @@
 
           <param name="can_interface_name">panther_can</param>
           <param name="master_can_id">3</param>
-          <param name="front_driver_can_id">1</param>
-          <param name="rear_driver_can_id">2</param>
+          <param name="driver_can_id">1</param>
           <param name="sdo_operation_timeout_ms">100</param>
 
           <!-- Depends on frequency of the controller, more critical motors state are sent with
diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt
index 43a333eb..1728bd37 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/panther_hardware_interfaces/CMakeLists.txt
@@ -56,13 +56,17 @@ add_library(
   src/panther_system/gpio/gpio_controller.cpp
   src/panther_system/gpio/gpio_driver.cpp
   src/panther_system/robot_driver/canopen_manager.cpp
-  src/panther_system/robot_driver/robot_driver.cpp
   src/panther_system/robot_driver/roboteq_data_converters.cpp
   src/panther_system/robot_driver/roboteq_driver.cpp
   src/panther_system/robot_driver/roboteq_error_filter.cpp
-  src/panther_system/panther_system_e_stop.cpp
-  src/panther_system/panther_system_ros_interface.cpp
+  src/panther_system/robot_driver/roboteq_robot_driver.cpp
+  src/panther_system/robot_driver/lynx_robot_driver.cpp
+  src/panther_system/robot_driver/panther_robot_driver.cpp
+  src/panther_system/system_e_stop.cpp
+  src/panther_system/system_ros_interface.cpp
+  src/panther_system/lynx_system.cpp
   src/panther_system/panther_system.cpp
+  src/panther_system/ugv_system.cpp
   src/utils.cpp)
 ament_target_dependencies(${PROJECT_NAME} ${PACKAGE_DEPENDENCIES})
 target_link_libraries(${PROJECT_NAME} PkgConfig::LIBLELY_COAPP
@@ -164,39 +168,39 @@ if(BUILD_TESTING)
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gmock(
-    ${PROJECT_NAME}_test_panther_robot_driver
-    test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
+    ${PROJECT_NAME}_test_lynx_robot_driver
+    test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
     src/panther_system/robot_driver/canopen_manager.cpp
     src/panther_system/robot_driver/roboteq_driver.cpp
     src/panther_system/robot_driver/roboteq_data_converters.cpp
     src/panther_system/robot_driver/roboteq_robot_driver.cpp
-    src/panther_system/robot_driver/panther_robot_driver.cpp
+    src/panther_system/robot_driver/lynx_robot_driver.cpp
     src/utils.cpp)
   target_include_directories(
-    ${PROJECT_NAME}_test_panther_robot_driver
+    ${PROJECT_NAME}_test_lynx_robot_driver
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
-  ament_target_dependencies(${PROJECT_NAME}_test_panther_robot_driver rclcpp
+  ament_target_dependencies(${PROJECT_NAME}_test_lynx_robot_driver rclcpp
                             panther_msgs panther_utils)
-  target_link_libraries(${PROJECT_NAME}_test_panther_robot_driver
+  target_link_libraries(${PROJECT_NAME}_test_lynx_robot_driver
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gmock(
-    ${PROJECT_NAME}_test_lynx_robot_driver
-    test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
+    ${PROJECT_NAME}_test_panther_robot_driver
+    test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
     src/panther_system/robot_driver/canopen_manager.cpp
     src/panther_system/robot_driver/roboteq_driver.cpp
     src/panther_system/robot_driver/roboteq_data_converters.cpp
     src/panther_system/robot_driver/roboteq_robot_driver.cpp
-    src/panther_system/robot_driver/lynx_robot_driver.cpp
+    src/panther_system/robot_driver/panther_robot_driver.cpp
     src/utils.cpp)
   target_include_directories(
-    ${PROJECT_NAME}_test_lynx_robot_driver
+    ${PROJECT_NAME}_test_panther_robot_driver
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
-  ament_target_dependencies(${PROJECT_NAME}_test_lynx_robot_driver rclcpp
+  ament_target_dependencies(${PROJECT_NAME}_test_panther_robot_driver rclcpp
                             panther_msgs panther_utils)
-  target_link_libraries(${PROJECT_NAME}_test_lynx_robot_driver
+  target_link_libraries(${PROJECT_NAME}_test_panther_robot_driver
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gmock(
@@ -209,30 +213,104 @@ if(BUILD_TESTING)
                         PkgConfig::LIBGPIOD)
 
   ament_add_gtest(
-    ${PROJECT_NAME}_test_panther_system_ros_interface
-    test/panther_system/test_panther_system_ros_interface.cpp
-    src/panther_system/panther_system_ros_interface.cpp
+    ${PROJECT_NAME}_test_system_ros_interface
+    test/unit/panther_system/test_system_ros_interface.cpp
+    src/panther_system/system_ros_interface.cpp
     src/panther_system/robot_driver/roboteq_data_converters.cpp
-    src/utils.cpp
     src/panther_system/gpio/gpio_controller.cpp
-    src/panther_system/gpio/gpio_driver.cpp)
+    src/utils.cpp)
   target_include_directories(
-    ${PROJECT_NAME}_test_panther_system_ros_interface
+    ${PROJECT_NAME}_test_system_ros_interface
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(
-    ${PROJECT_NAME}_test_panther_system_ros_interface
+    ${PROJECT_NAME}_test_system_ros_interface
     diagnostic_updater
     rclcpp
     panther_msgs
     panther_utils
     realtime_tools
     std_srvs)
-  target_link_libraries(${PROJECT_NAME}_test_panther_system_ros_interface
+  target_link_libraries(${PROJECT_NAME}_test_system_ros_interface
+                        PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD)
+
+  ament_add_gmock(
+    ${PROJECT_NAME}_test_ugv_system
+    test/unit/panther_system/test_ugv_system.cpp
+    src/panther_system/gpio/gpio_controller.cpp
+    src/panther_system/gpio/gpio_driver.cpp
+    src/panther_system/system_e_stop.cpp
+    src/panther_system/robot_driver/roboteq_data_converters.cpp
+    src/panther_system/robot_driver/roboteq_error_filter.cpp
+    src/panther_system/system_ros_interface.cpp
+    src/panther_system/ugv_system.cpp
+    src/utils.cpp)
+  set_tests_properties(${PROJECT_NAME}_test_ugv_system PROPERTIES TIMEOUT 120)
+  target_include_directories(
+    ${PROJECT_NAME}_test_ugv_system
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(
+    ${PROJECT_NAME}_test_ugv_system
+    diagnostic_updater
+    hardware_interface
+    rclcpp
+    panther_msgs
+    panther_utils
+    std_msgs
+    std_srvs)
+  target_link_libraries(${PROJECT_NAME}_test_ugv_system
+                        PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD)
+
+  ament_add_gmock(
+    ${PROJECT_NAME}_test_lynx_system
+    test/unit/panther_system/test_lynx_system.cpp
+    src/panther_system/gpio/gpio_controller.cpp
+    src/panther_system/gpio/gpio_driver.cpp
+    src/panther_system/system_e_stop.cpp
+    src/panther_system/robot_driver/canopen_manager.cpp
+    src/panther_system/robot_driver/roboteq_robot_driver.cpp
+    src/panther_system/robot_driver/roboteq_data_converters.cpp
+    src/panther_system/robot_driver/roboteq_driver.cpp
+    src/panther_system/robot_driver/roboteq_error_filter.cpp
+    src/panther_system/robot_driver/lynx_robot_driver.cpp
+    src/panther_system/system_ros_interface.cpp
+    src/panther_system/lynx_system.cpp
+    src/panther_system/ugv_system.cpp
+    src/utils.cpp)
+  set_tests_properties(${PROJECT_NAME}_test_lynx_system PROPERTIES TIMEOUT 120)
+  target_include_directories(
+    ${PROJECT_NAME}_test_lynx_system
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(
+    ${PROJECT_NAME}_test_lynx_system
+    diagnostic_updater
+    hardware_interface
+    rclcpp
+    panther_msgs
+    panther_utils
+    std_msgs
+    std_srvs)
+  target_link_libraries(${PROJECT_NAME}_test_lynx_system
                         PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD)
 
-  ament_add_gtest(${PROJECT_NAME}_test_panther_system
-                  test/panther_system/test_panther_system.cpp)
+  ament_add_gmock(
+    ${PROJECT_NAME}_test_panther_system
+    test/unit/panther_system/test_panther_system.cpp
+    src/panther_system/gpio/gpio_controller.cpp
+    src/panther_system/gpio/gpio_driver.cpp
+    src/panther_system/system_e_stop.cpp
+    src/panther_system/robot_driver/canopen_manager.cpp
+    src/panther_system/robot_driver/roboteq_robot_driver.cpp
+    src/panther_system/robot_driver/roboteq_data_converters.cpp
+    src/panther_system/robot_driver/roboteq_driver.cpp
+    src/panther_system/robot_driver/roboteq_error_filter.cpp
+    src/panther_system/robot_driver/panther_robot_driver.cpp
+    src/panther_system/system_ros_interface.cpp
+    src/panther_system/panther_system.cpp
+    src/panther_system/ugv_system.cpp
+    src/utils.cpp)
   set_tests_properties(${PROJECT_NAME}_test_panther_system PROPERTIES TIMEOUT
                                                                       120)
   target_include_directories(
@@ -240,15 +318,19 @@ if(BUILD_TESTING)
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(
-    ${PROJECT_NAME}_test_panther_system hardware_interface rclcpp panther_msgs
-    panther_utils)
+    ${PROJECT_NAME}_test_panther_system
+    diagnostic_updater
+    hardware_interface
+    rclcpp
+    panther_msgs
+    panther_utils
+    std_msgs
+    std_srvs)
   target_link_libraries(${PROJECT_NAME}_test_panther_system
-                        PkgConfig::LIBLELY_COAPP)
+                        PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD)
 
-  # Integration tests
-  option(TEST_INTEGRATION "Run integration tests" OFF)
-  if(TEST_INTEGRATION)
-    # Hardware integration
+  # Integration tests option(TEST_INTEGRATION "Run integration tests" OFF)
+  if(TEST_INTEGRATION) # Hardware integration
     ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver
                     test/panther_system/gpio/test_gpio_driver.cpp)
     ament_target_dependencies(${PROJECT_NAME}_test_gpiod_driver panther_utils)
diff --git a/panther_hardware_interfaces/README.md b/panther_hardware_interfaces/README.md
index bd1b7bf3..545dd777 100644
--- a/panther_hardware_interfaces/README.md
+++ b/panther_hardware_interfaces/README.md
@@ -6,13 +6,13 @@ Package that implements SystemInterface from ros2_control for Panther.
 
 This package doesn't contain any standalone nodes, only plugins that are loaded by the resource manager. To use this hardware interface, add it to your URDF. You can check how to do it in [panther_description](https://github.com/husarion/panther_ros/tree/ros2/panther_description/).
 
-### PantherSystem
+### UGVSystem (PantherSystem | LynxSystem)
 
-Plugin responsible for communicating with engine controllers via the CAN bus, providing E-Stop functionalities and managing Built-in Computer GPIO ports.
+Plugins for Panther and Lynx are based on an abstraction called UGVSystem. Most parts of both systems are similar and are described below. Each plugin is responsible for communicating with engine controllers via the CAN bus, providing E-Stop functionality, and managing the built-in computer's GPIO ports.
 
 #### Publishers
 
-- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Panther system diagnostic messages.
+- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: System diagnostic messages.
 - `hardware/e_stop` [*std_msgs/Bool*]: Current E-stop state.
 - `hardware/io_state` [*panther_msgs/IOState*]: Current IO state.
 - `hardware/robot_driver_state` [*panther_msgs/RobotDriverState*]: Current motor controllers' state and error flags.
@@ -30,7 +30,7 @@ Plugin responsible for communicating with engine controllers via the CAN bus, pr
 
 #### Parameters
 
-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)).
+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) or [lynx_macro.urdf.xacro](../lynx_description/urdf/lynx_macro.urdf.xacro)).
 
 Physical properties
 
@@ -44,8 +44,6 @@ CAN settings
 
 - `can_interface_name` [*string*, default: **panther_can**]: Name of the CAN interface.
 - `master_can_id` [*int*, default: **3**]: CAN ID of the master device (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).
-- `front_driver_can_id` [*int*, default: **1**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).
-- `rear_driver_can_id` [*int*, default: **2**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).
 - `sdo_operation_timeout_ms` [*int*, default: **100**]: Timeout of the SDO operations, currently no SDO operation is required in RT operation, so this timeout can be set to a higher value.
 - `pdo_motor_states_timeout_ms` [*int*, default: **15**]: Depends on the frequency at which Roboteq is configured to send motor states (PDO 1 and 2) data. By default, there should be 10 **[ms]** between received data, if it takes more than `pdo_motor_states_timeout_ms`, a motor states read error is triggered. The default value is set to be expected period +50% margin.
 - `pdo_driver_state_timeout_ms` [*int*, default: **75**]: Depends on the frequency at which Roboteq is configured to send driver state (PDO 3 and 4) data. By default, there should be 50 **[ms]** between received data, if it takes more than `pdo_driver_state_timeout_ms`, a driver state read error is triggered. The default value is set to be expected period +50% margin.
@@ -56,6 +54,15 @@ CAN settings
 - `max_read_pdo_motor_states_errors_count` [*int*, default: **2**]: How many consecutive errors can happen before escalating to general error.
 - `max_read_pdo_driver_state_errors_count` [*int*, default: **2**]: How many consecutive errors can happen before escalating to general error.
 
+PantherSystem additional CAN settings
+
+- `front_driver_can_id` [*int*, default: **1**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).
+- `rear_driver_can_id` [*int*, default: **2**, **Required only for PantherSystem**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).
+
+LynxSystem additional CAN settings
+
+- `driver_can_id` [*int*, default: **1**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).
+
 > [!CAUTION]
 > `max_write_pdo_cmds_errors_count`, `max_read_pdo_motor_states_errors_count`, `max_read_pdo_driver_state_errors_count`, `sdo_operation_timeout`, `pdo_motor_states_timeout_ms` and `pdo_driver_state_timeout_ms` are safety-critical parameters, they should be changed only in very specific cases, be sure that you know how they work and be really cautious when changing them.
 
@@ -141,17 +148,11 @@ To configure RT check out the instructions provided in the [ros2_control docs](h
 
 ### Setup
 
-First, it is necessary to set up a virtual CAN:
-
-<!-- todo move setup somewhere so the test can be run more easily -->
+Since some of the tests create and access a virtual CAN interface, it is necessary to install `kmod` and `iproute2`.
 
 ```bash
-sudo modprobe vcan
-sudo ip link add dev panther_can type vcan
-sudo ip link set up panther_can
-sudo ip link set panther_can down
-sudo ip link set panther_can txqueuelen 1000
-sudo ip link set panther_can up
+sudo apt update
+sudo apt install -y kmod iproute2
 ```
 
 ### Running tests
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/lynx_system.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/lynx_system.hpp
new file mode 100644
index 00000000..b5927e59
--- /dev/null
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/lynx_system.hpp
@@ -0,0 +1,60 @@
+// 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_HARDWARE_INTERFACES_PANTHER_SYSTEM_LYNX_SYSTEM_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_LYNX_SYSTEM_HPP_
+
+#include <string>
+#include <vector>
+
+#include "panther_hardware_interfaces/panther_system/ugv_system.hpp"
+
+namespace panther_hardware_interfaces
+{
+
+/**
+ * @brief Class that implements UGVSystem for Lynx robot
+ */
+class LynxSystem : public UGVSystem
+{
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(LynxSystem)
+
+  LynxSystem() : UGVSystem(kJointOrder) {}
+
+  ~LynxSystem() = default;
+
+protected:
+  void ReadCANopenSettingsDriverCANIDs() override;
+
+  virtual void DefineRobotDriver() override;  // virtual for testing
+
+  void UpdateHwStates() override;
+  void UpdateMotorsStateDataTimedOut() override;
+
+  void UpdateDriverStateMsg() override;
+  void UpdateFlagErrors() override;
+  void UpdateDriverStateDataTimedOut() override;
+
+  std::vector<float> GetSpeedCommands() const;
+
+  void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) override;
+  void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) override;
+
+  static const inline std::vector<std::string> kJointOrder = {"fl", "fr", "rl", "rr"};
+};
+
+}  // namespace panther_hardware_interfaces
+
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_LYNX_SYSTEM_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp
index 373ff0ee..80cf06bb 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp
@@ -15,141 +15,44 @@
 #ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_HPP_
 #define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_HPP_
 
-#include <array>
-#include <functional>
-#include <memory>
 #include <string>
 #include <vector>
 
-#include "diagnostic_updater/diagnostic_status_wrapper.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
-#include "rclcpp_lifecycle/state.hpp"
-
-#include "hardware_interface/handle.hpp"
-#include "hardware_interface/system_interface.hpp"
-#include "hardware_interface/types/hardware_interface_return_values.hpp"
-
-#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp"
-#include "panther_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp"
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp"
-#include "panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp"
-#include "panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp"
+#include "panther_hardware_interfaces/panther_system/ugv_system.hpp"
 
 namespace panther_hardware_interfaces
 {
 
-using return_type = hardware_interface::return_type;
-using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
-using StateInterface = hardware_interface::StateInterface;
-using CommandInterface = hardware_interface::CommandInterface;
-
 /**
- * @brief Class that implements SystemInterface from ros2_control for Panther
+ * @brief Class that implements UGVSystem for Panther robot
  */
-class PantherSystem : public hardware_interface::SystemInterface
+class PantherSystem : public UGVSystem
 {
 public:
   RCLCPP_SHARED_PTR_DEFINITIONS(PantherSystem)
 
-  CallbackReturn on_init(const hardware_interface::HardwareInfo & hardware_info) override;
-  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
-  CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
-  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
-  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
-  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
-  CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override;
+  PantherSystem() : UGVSystem(kJointOrder) {}
 
-  std::vector<StateInterface> export_state_interfaces() override;
-  std::vector<CommandInterface> export_command_interfaces() override;
-
-  return_type read(const rclcpp::Time & time, const rclcpp::Duration & /* period */) override;
-  return_type write(
-    const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) override;
+  ~PantherSystem() = default;
 
 protected:
-  void CheckJointSize() const;
-  void SortAndCheckJointNames();
-  void SetInitialValues();
-  void CheckInterfaces() const;
-  void ReadPantherVersion();
-  void ReadDrivetrainSettings();
-  void ReadCANopenSettings();
-  void ReadInitializationActivationAttempts();
-  void ReadParametersAndCreateRoboteqErrorFilter();
-  void ReadDriverStatesUpdateFrequency();
-
-  void ConfigureGPIOController();
-  void ConfigureMotorsController();
-  void ConfigureEStop();
-
-  void UpdateMotorsState();
-  void UpdateDriverState();
-  void UpdateEStopState();
-
-  void UpdateHwStates();
-  void UpdateMotorsStateDataTimedOut();
-  bool AreVelocityCommandsNearZero();
-
-  void UpdateDriverStateMsg();
-  void UpdateFlagErrors();
-  void UpdateDriverStateDataTimedOut();
-
-  void HandlePDOWriteOperation(std::function<void()> pdo_write_operation);
-
-  void MotorsPowerEnable(const bool enable);
-
-  void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status);
-  void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status);
-
-  static constexpr size_t kJointsSize = 4;
-
-  // Currently only velocity command mode is supported - although Roboteq driver support position
-  // and torque mode, in 2.1 firmware both modes aren't really stable and safe.
-  // In torque mode sometimes after killing the software motor moves and it generally isn't well
-  // tuned. Position mode also isn't really stable (reacts abruptly to spikes). If updating the
-  // firmware to 2.1a will solve these issues, it may be worth to enable other modes.
-  std::array<double, kJointsSize> hw_commands_velocities_;
-
-  std::array<double, kJointsSize> hw_states_positions_;
-  std::array<double, kJointsSize> hw_states_velocities_;
-  std::array<double, kJointsSize> hw_states_efforts_;
-
-  // Define expected joint order, so that it doesn't matter what order is defined in the URDF.
-  // It is expected that the joint name should contain these specifiers.
-  static const inline std::array<std::string, kJointsSize> joint_order_ = {"fl", "fr", "rl", "rr"};
-  std::array<std::string, kJointsSize> joints_names_sorted_;
-
-  std::shared_ptr<GPIOControllerInterface> gpio_controller_;
-  std::shared_ptr<MotorsController> motors_controller_;
-  std::shared_ptr<EStopInterface> e_stop_;
-
-  DrivetrainSettings drivetrain_settings_;
-  CANopenSettings canopen_settings_;
-
-  std::unique_ptr<PantherSystemRosInterface> panther_system_ros_interface_;
+  void ReadCANopenSettingsDriverCANIDs() override;
 
-  // Sometimes SDO errors can happen during initialization and activation of Roboteq drivers,
-  // in these cases it is better to retry
-  // Example errors:
-  // SDO abort code 05040000 received on upload request of object 1000 (Device type) to
-  // node 02: SDO protocol timed out
-  // SDO abort code 05040000 received on upload request of sub-object 1018:01 (Vendor-ID) to
-  // node 02: SDO protocol timed out
-  unsigned max_roboteq_initialization_attempts_;
-  unsigned max_roboteq_activation_attempts_;
+  virtual void DefineRobotDriver() override;  // virtual for testing
 
-  rclcpp::Logger logger_{rclcpp::get_logger("PantherSystem")};
-  rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
+  void UpdateHwStates() override;
+  void UpdateMotorsStateDataTimedOut() override;
 
-  std::shared_ptr<RoboteqErrorFilter> roboteq_error_filter_;
+  void UpdateDriverStateMsg() override;
+  void UpdateFlagErrors() override;
+  void UpdateDriverStateDataTimedOut() override;
 
-  float panther_version_;
+  std::vector<float> GetSpeedCommands() const;
 
-  std::shared_ptr<std::mutex> motor_controller_write_mtx_;
+  void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) override;
+  void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) override;
 
-  rclcpp::Time next_driver_state_update_time_{0, 0, RCL_ROS_TIME};
-  rclcpp::Duration driver_states_update_period_{0, 0};
+  static const inline std::vector<std::string> kJointOrder = {"fl", "fr", "rl", "rr"};
 };
 
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp
deleted file mode 100644
index 146bd909..00000000
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp
+++ /dev/null
@@ -1,191 +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_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_E_STOP_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_E_STOP_HPP_
-
-#include <atomic>
-#include <memory>
-#include <mutex>
-
-#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp"
-#include "panther_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp"
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp"
-
-namespace panther_hardware_interfaces
-{
-
-/**
- * @class EStopInterface
- * @brief Abstract base class defining the interface for emergency stop detailed implementations.
- */
-class EStopInterface
-{
-public:
-  EStopInterface(
-    std::shared_ptr<GPIOControllerInterface> gpio_controller,
-    std::shared_ptr<RoboteqErrorFilter> roboteq_error_filter,
-    std::shared_ptr<MotorsController> motors_controller,
-    std::shared_ptr<std::mutex> motor_controller_write_mtx,
-    std::function<bool()> zero_velocity_check)
-  : gpio_controller_(gpio_controller),
-    roboteq_error_filter_(roboteq_error_filter),
-    motors_controller_(motors_controller),
-    motor_controller_write_mtx_(motor_controller_write_mtx),
-    ZeroVelocityCheck(zero_velocity_check) {};
-
-  virtual ~EStopInterface() = default;
-
-  virtual bool ReadEStopState() = 0;
-  virtual void TriggerEStop() = 0;
-  virtual void ResetEStop() = 0;
-
-protected:
-  std::shared_ptr<GPIOControllerInterface> gpio_controller_;
-  std::shared_ptr<RoboteqErrorFilter> roboteq_error_filter_;
-  std::shared_ptr<MotorsController> motors_controller_;
-  std::shared_ptr<std::mutex> motor_controller_write_mtx_;
-
-  std::function<bool()> ZeroVelocityCheck;
-
-  std::mutex e_stop_manipulation_mtx_;
-  std::atomic_bool e_stop_triggered_ = true;
-};
-
-/**
- * @class EStopPTH12X
- * @brief Implements the emergency stop for the PTH12X hardware variant.
- */
-class EStopPTH12X : public EStopInterface
-{
-public:
-  EStopPTH12X(
-    std::shared_ptr<GPIOControllerInterface> gpio_controller,
-    std::shared_ptr<RoboteqErrorFilter> roboteq_error_filter,
-    std::shared_ptr<MotorsController> motors_controller,
-    std::shared_ptr<std::mutex> motor_controller_write_mtx,
-    std::function<bool()> zero_velocity_check)
-  : EStopInterface(
-      gpio_controller, roboteq_error_filter, motors_controller, motor_controller_write_mtx,
-      zero_velocity_check) {};
-
-  virtual ~EStopPTH12X() override = default;
-
-  /**
-   * @brief Checks the emergency stop state.
-   *
-   *   1. Check if ESTOP GPIO pin is not active. If is not it means that E-Stop is triggered by
-   *      another device within the robot's system (e.g., Roboteq controller or Safety Board),
-   *      disabling the software Watchdog is necessary to prevent an uncontrolled reset.
-   *   2. If there is a need, disable software Watchdog using
-   *      GPIOControllerInterface::EStopTrigger method.
-   *   3. Return ESTOP GPIO pin state.
-   */
-  bool ReadEStopState() override;
-
-  /**
-   * @brief Triggers the emergency stop.
-   *
-   *   1. Interrupt the E-Stop resetting process if it is in progress.
-   *   2. Attempt to trigger the E-Stop using GPIO by disabling the software-controlled watchdog.
-   *   3. If successful, set e_stop_triggered_ to true; otherwise, throw a std::runtime_error
-   *      exception.
-   *
-   * @throws std::runtime_error if triggering the E-stop using GPIO fails.
-   */
-  void TriggerEStop() override;
-
-  /**
-   * @brief Resets the emergency stop.
-   *
-   *   1. Verify that the last velocity commands are zero to prevent an uncontrolled robot movement
-   *      after an E-stop state change.
-   *   2. Attempt to reset the E-Stop using GPIO by manipulating the ESTOP GPIO pin. This operation
-   *      may take some time and can be interrupted by the E-Stop trigger process.
-   *   3. Set the clear_error flag to allow for clearing of Roboteq errors.
-   *   4. Confirm the E-Stop reset was successful with the ReadEStopState method.
-   *
-   * @throws EStopResetInterrupted if the E-stop reset operation was halted because the E-stop was
-   *         triggered again.
-   * @throws std::runtime_error if an error occurs when trying to reset the E-stop using GPIO.
-   */
-  void ResetEStop() override;
-};
-
-/**
- * @class EStopPTH10X
- * @brief Implements the emergency stop for the PTH10X hardware variant. In this robot
- * version, only a software-based E-Stop is supported. There are no hardware components that
- * implement E-Stop functionality.
- */
-class EStopPTH10X : public EStopInterface
-{
-public:
-  EStopPTH10X(
-    std::shared_ptr<GPIOControllerInterface> gpio_controller,
-    std::shared_ptr<RoboteqErrorFilter> roboteq_error_filter,
-    std::shared_ptr<MotorsController> motors_controller,
-    std::shared_ptr<std::mutex> motor_controller_write_mtx,
-    std::function<bool()> zero_velocity_check)
-  : EStopInterface(
-      gpio_controller, roboteq_error_filter, motors_controller, motor_controller_write_mtx,
-      zero_velocity_check) {};
-
-  virtual ~EStopPTH10X() override = default;
-
-  /**
-   * @brief Checks the emergency stop state.
-   *
-   *  1. Verify if the main switch is in the STAGE2 position to confirm if the motors are powered
-   *     up.
-   *  2. Check for any errors reported by the Roboteq controller.
-   *  3. If the E-Stop is not currently triggered, initiate an E-Stop if the motors are not powered
-   *     up or if a driver error has occurred.
-   *  4. Return the actual state of the E-Stop.
-   *
-   * @return A boolean indicating whether the E-Stop is currently triggered `true` or not `false`.
-   */
-  bool ReadEStopState() override;
-
-  /**
-   * @brief Trigger the emergency stop state.
-   *
-   *  1. Send a command to the Roboteq controllers to enable the Safety Stop.
-   *     Note: The Safety Stop is a specific state of the Roboteq controller, distinct from
-   *     the E-Stop state of the Panther robot.
-   *  2. Update the e_stop_triggered_ flag to indicate that the E-Stop state has been triggered.
-   */
-  void TriggerEStop() override;
-
-  /**
-   * @brief Resets the emergency stop.
-   *
-   *   1. Verify that the last velocity commands are zero to prevent an uncontrolled robot movement
-   *      after an E-stop state change.
-   *   2. Verify if the main switch is in the STAGE2 position to confirm if the motors are powered
-   *      up.
-   *   3. Check for any errors reported by the Roboteq controller.
-   *   4. Set the clear_error flag to allow for clearing of Roboteq errors.
-   *   5. Confirm the E-Stop reset was successful with the ReadEStopState method.
-   *   6. Update the e_stop_triggered_ flag to indicate that the E-Stop state has been triggered.
-   *
-   * @throws std::runtime_error if the E-stop reset operation was halted because the E-stop was
-   *         triggered again,  motors are not powered or motor controller is in an error state.
-   */
-  void ResetEStop() override;
-};
-
-}  // namespace panther_hardware_interfaces
-
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_E_STOP_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
index 6449d7c6..25b61114 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
@@ -63,7 +63,7 @@ class LynxRobotDriver : public RoboteqRobotDriver
    * @brief This method defines driver objects and adds motor drivers for them. It is virtual to
    * allow mocking drivers in tests.
    */
-  virtual void DefineDrivers();
+  void DefineDrivers() override;
 };
 
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
index bccd1f64..78e38064 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
@@ -107,6 +107,13 @@ class RobotDriverInterface
    * the error flags should remain active.
    */
   virtual void AttemptErrorFlagReset() = 0;
+
+  /**
+   * @brief Check if communication with drivers is working properly
+   *
+   * @return true if communication error occurred
+   */
+  virtual bool CommunicationError() = 0;
 };
 
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
index 3d4e8440..6f872307 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
@@ -132,6 +132,8 @@ class RoboteqRobotDriver : public RobotDriverInterface
    */
   const DriverData & GetData(const std::string & name) override;
 
+  bool CommunicationError() override;
+
 protected:
   /**
    * @brief This method defines driver objects and adds motor drivers for them.
@@ -156,6 +158,7 @@ class RoboteqRobotDriver : public RobotDriverInterface
   bool DataTimeout(
     const timespec & current_time, const timespec & data_timestamp,
     const std::chrono::milliseconds & timeout);
+  void BootDrivers();
 
   bool initialized_ = false;
 
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_e_stop.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_e_stop.hpp
new file mode 100644
index 00000000..a0d1088e
--- /dev/null
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_e_stop.hpp
@@ -0,0 +1,120 @@
+// 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_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_E_STOP_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_E_STOP_HPP_
+
+#include <atomic>
+#include <memory>
+#include <mutex>
+
+#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp"
+
+namespace panther_hardware_interfaces
+{
+
+/**
+ * @class EStopInterface
+ * @brief Abstract base class defining the interface for emergency stop detailed implementations.
+ */
+class EStopInterface
+{
+public:
+  EStopInterface() {}
+
+  virtual ~EStopInterface() = default;
+
+  virtual bool ReadEStopState() = 0;
+  virtual void TriggerEStop() = 0;
+  virtual void ResetEStop() = 0;
+};
+
+/**
+ * @class EStop
+ * @brief Implements the emergency stop interface.
+ */
+class EStop : public EStopInterface
+{
+public:
+  EStop(
+    std::shared_ptr<GPIOControllerInterface> gpio_controller,
+    std::shared_ptr<RoboteqErrorFilter> roboteq_error_filter,
+    std::shared_ptr<RobotDriverInterface> robot_driver,
+    std::shared_ptr<std::mutex> robot_driver_write_mtx, std::function<bool()> zero_velocity_check)
+  : EStopInterface(),
+    gpio_controller_(gpio_controller),
+    roboteq_error_filter_(roboteq_error_filter),
+    robot_driver_(robot_driver),
+    robot_driver_write_mtx_(robot_driver_write_mtx),
+    ZeroVelocityCheck(zero_velocity_check) {};
+
+  virtual ~EStop() override = default;
+
+  /**
+   * @brief Checks the emergency stop state.
+   *
+   *   1. Check if ESTOP GPIO pin is not active. If is not it means that E-Stop is triggered by
+   *      another device within the robot's system (e.g., Roboteq controller or Safety Board),
+   *      disabling the software Watchdog is necessary to prevent an uncontrolled reset.
+   *   2. If there is a need, disable software Watchdog using
+   *      GPIOControllerInterface::EStopTrigger method.
+   *   3. Return ESTOP GPIO pin state.
+   */
+  bool ReadEStopState() override;
+
+  /**
+   * @brief Triggers the emergency stop.
+   *
+   *   1. Interrupt the E-Stop resetting process if it is in progress.
+   *   2. Attempt to trigger the E-Stop using GPIO by disabling the software-controlled watchdog.
+   *   3. If successful, set e_stop_triggered_ to true; otherwise, throw a std::runtime_error
+   *      exception.
+   *
+   * @throws std::runtime_error if triggering the E-stop using GPIO fails.
+   */
+  void TriggerEStop() override;
+
+  /**
+   * @brief Resets the emergency stop.
+   *
+   *   1. Verify that the last velocity commands are zero to prevent an uncontrolled robot movement
+   *      after an E-stop state change.
+   *   2. Attempt to reset the E-Stop using GPIO by manipulating the ESTOP GPIO pin. This operation
+   *      may take some time and can be interrupted by the E-Stop trigger process.
+   *   3. Set the clear_error flag to allow for clearing of Roboteq errors.
+   *   4. Confirm the E-Stop reset was successful with the ReadEStopState method.
+   *
+   * @throws EStopResetInterrupted if the E-stop reset operation was halted because the E-stop was
+   *         triggered again.
+   * @throws std::runtime_error if an error occurs when trying to reset the E-stop using GPIO.
+   */
+  void ResetEStop() override;
+
+protected:
+  std::shared_ptr<GPIOControllerInterface> gpio_controller_;
+  std::shared_ptr<RoboteqErrorFilter> roboteq_error_filter_;
+  std::shared_ptr<RobotDriverInterface> robot_driver_;
+  std::shared_ptr<std::mutex> robot_driver_write_mtx_;
+
+  std::function<bool()> ZeroVelocityCheck;
+
+  std::mutex e_stop_manipulation_mtx_;
+  std::atomic_bool e_stop_triggered_ = true;
+};
+
+}  // namespace panther_hardware_interfaces
+
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_E_STOP_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_ros_interface.hpp
similarity index 85%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp
rename to panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_ros_interface.hpp
index ca04943d..856d4b72 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_ros_interface.hpp
@@ -12,30 +12,30 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_ROS_INTERFACE_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_ROS_INTERFACE_HPP_
+#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_
 
 #include <any>
-#include <atomic>
 #include <functional>
+#include <map>
 #include <memory>
 #include <thread>
 #include <unordered_map>
 
-#include "diagnostic_updater/diagnostic_updater.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "realtime_tools/realtime_publisher.h"
+#include <realtime_tools/realtime_publisher.h>
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
 
-#include "std_msgs/msg/bool.hpp"
-#include "std_srvs/srv/set_bool.hpp"
-#include "std_srvs/srv/trigger.hpp"
+#include <std_msgs/msg/bool.hpp>
+#include <std_srvs/srv/set_bool.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
 #include "panther_msgs/msg/driver_state_named.hpp"
 #include "panther_msgs/msg/io_state.hpp"
 #include "panther_msgs/msg/robot_driver_state.hpp"
 
 #include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp"
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
 
 using namespace std::placeholders;
 
@@ -49,6 +49,14 @@ using DriverStateNamedMsg = panther_msgs::msg::DriverStateNamed;
 using SetBoolSrv = std_srvs::srv::SetBool;
 using TriggerSrv = std_srvs::srv::Trigger;
 
+struct DriverCANErrors
+{
+  bool motor_states_data_timed_out;
+  bool driver_state_data_timed_out;
+  bool can_error;
+  bool heartbeat_timeout;
+};
+
 struct CANErrors
 {
   bool error;
@@ -57,17 +65,7 @@ struct CANErrors
   bool read_pdo_motor_states_error;
   bool read_pdo_driver_state_error;
 
-  bool front_motor_states_data_timed_out;
-  bool rear_motor_states_data_timed_out;
-
-  bool front_driver_state_data_timed_out;
-  bool rear_driver_state_data_timed_out;
-
-  bool front_can_error;
-  bool rear_can_error;
-
-  bool front_heartbeat_timeout;
-  bool rear_heartbeat_timeout;
+  std::unordered_map<std::string, DriverCANErrors> driver_errors;
 };
 
 /**
@@ -143,7 +141,7 @@ class ROSServiceWrapper
  * @brief Class that takes care of additional ROS interface of panther system, such as publishing
  * driver state and providing service for clearing errors
  */
-class PantherSystemRosInterface
+class SystemROSInterface
 {
 public:
   /**
@@ -152,10 +150,10 @@ class PantherSystemRosInterface
    * @param node_name
    * @param node_options
    */
-  PantherSystemRosInterface(
+  SystemROSInterface(
     const std::string & node_name,
     const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions());
-  ~PantherSystemRosInterface();
+  ~SystemROSInterface();
 
   /**
    * @brief Registers a new service server associated with a specific service type and callback on
@@ -222,14 +220,21 @@ class PantherSystemRosInterface
   }
 
   /**
-   * @brief Updates fault flags, script flags, and runtime errors in the driver state msg
+   * @brief Updates fault flags, script flags, and runtime errors in the robot driver state msg
+   *
+   * @param name The name of the driver to update the flags for
+   * @param data The data to update the flags with
    */
-  void UpdateMsgErrorFlags(const RoboteqData & front, const RoboteqData & rear);
+  void UpdateMsgErrorFlags(const std::string & name, const DriverData & data);
 
   /**
-   * @brief Updates parameters of the drivers: voltage, current and temperature
+   * @brief Updates parameters of the driver (voltage, current and temperature) in robot driver
+   * state msg
+   *
+   * @param name The name of the driver to update the parameters for
+   * @param state The data to update the parameters with
    */
-  void UpdateMsgDriversStates(const DriverState & front, const DriverState & rear);
+  void UpdateMsgDriversStates(const std::string & name, const RoboteqDriverState & state);
 
   /**
    * @brief Updates the current state of communication errors and general error state
@@ -242,7 +247,7 @@ class PantherSystemRosInterface
   void InitializeAndPublishIOStateMsg(const std::unordered_map<GPIOPin, bool> & io_state);
   void PublishIOState(const GPIOInfo & gpio_info);
 
-private:
+protected:
   /**
    * @brief Updates the IOState message and indicates whether its state has changed.
    *
@@ -271,8 +276,15 @@ class PantherSystemRosInterface
   rclcpp::CallbackGroup::SharedPtr GetOrCreateNodeCallbackGroup(
     const unsigned group_id, rclcpp::CallbackGroupType callback_group_type);
 
-  void InitializeRobotDriverStateMsg();
-
+  /**
+   * @brief Retrieves the driver state message associated with the specified name. If the driver
+   * state is not found, it is created.
+   *
+   * @param robot_driver_state The robot driver state message to retrieve driver state from.
+   * @param name The name of the driver state to be retrieved.
+   *
+   * @return Reference to the driver state message associated with the specified name.
+   */
   DriverStateNamedMsg & GetDriverStateByName(
     RobotDriverStateMsg & robot_driver_state, const std::string & name);
 
@@ -298,4 +310,4 @@ class PantherSystemRosInterface
 
 }  // namespace panther_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_ROS_INTERFACE_HPP_
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/ugv_system.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/ugv_system.hpp
new file mode 100644
index 00000000..03fd02b5
--- /dev/null
+++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/ugv_system.hpp
@@ -0,0 +1,168 @@
+// 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_HARDWARE_INTERFACES_PANTHER_SYSTEM_UGV_SYSTEM_HPP_
+#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_UGV_SYSTEM_HPP_
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <diagnostic_updater/diagnostic_status_wrapper.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp>
+#include <rclcpp_lifecycle/state.hpp>
+
+#include <hardware_interface/handle.hpp>
+#include <hardware_interface/system_interface.hpp>
+#include <hardware_interface/types/hardware_interface_return_values.hpp>
+
+#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp"
+#include "panther_hardware_interfaces/panther_system/system_e_stop.hpp"
+#include "panther_hardware_interfaces/panther_system/system_ros_interface.hpp"
+
+namespace panther_hardware_interfaces
+{
+
+using return_type = hardware_interface::return_type;
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+using StateInterface = hardware_interface::StateInterface;
+using CommandInterface = hardware_interface::CommandInterface;
+
+/**
+ * @brief Class that implements SystemInterface from ros2_control for Husarion UGV robots
+ */
+class UGVSystem : public hardware_interface::SystemInterface
+{
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(UGVSystem)
+  UGVSystem(const std::vector<std::string> & joint_order)
+  : SystemInterface(),
+    joint_size_(joint_order.size()),
+    joint_order_(joint_order),
+    joints_names_sorted_(joint_size_)
+  {
+  }
+
+  virtual ~UGVSystem() = default;
+
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & hardware_info) override;
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
+  CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override;
+
+  std::vector<StateInterface> export_state_interfaces() override;
+  std::vector<CommandInterface> export_command_interfaces() override;
+
+  return_type read(const rclcpp::Time & time, const rclcpp::Duration & /* period */) override;
+  return_type write(
+    const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) override;
+
+protected:
+  void CheckJointSize() const;
+  void SortAndCheckJointNames();
+  void SetInitialValues();
+  void CheckInterfaces() const;
+
+  void ReadDrivetrainSettings();
+  void ReadCANopenSettings();
+  virtual void ReadCANopenSettingsDriverCANIDs() = 0;
+
+  void ReadInitializationActivationAttempts();
+  void ReadParametersAndCreateRoboteqErrorFilter();
+  void ReadDriverStatesUpdateFrequency();
+
+  virtual void ConfigureGPIOController();  // virtual for mocking
+  void ConfigureRobotDriver();
+  virtual void DefineRobotDriver() = 0;
+  virtual void ConfigureEStop();  // virtual for mocking
+
+  void UpdateMotorsState();
+  void UpdateDriverState();
+  void UpdateEStopState();
+
+  virtual void UpdateHwStates() = 0;
+  virtual void UpdateMotorsStateDataTimedOut() = 0;  // possible but needs changes in robot driver
+  bool AreVelocityCommandsNearZero();
+
+  virtual void UpdateDriverStateMsg() = 0;
+  virtual void UpdateFlagErrors() = 0;               // possible but needs changes in robot driver
+  virtual void UpdateDriverStateDataTimedOut() = 0;  // possible but needs changes in robot driver
+
+  void HandleRobotDriverWriteOperation(std::function<void()> write_operation);
+  virtual std::vector<float> GetSpeedCommands() const = 0;
+
+  void MotorsPowerEnable(const bool enable);
+
+  virtual void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) = 0;
+  virtual void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) = 0;
+
+  const size_t joint_size_;
+
+  // Currently only velocity command mode is supported - although Roboteq driver support position
+  // and torque mode, in 2.1 firmware both modes aren't really stable and safe.
+  // In torque mode sometimes after killing the software motor moves and it generally isn't well
+  // tuned. Position mode also isn't really stable (reacts abruptly to spikes). If updating the
+  // firmware to 2.1a will solve these issues, it may be worth to enable other modes.
+  std::vector<double> hw_commands_velocities_;
+
+  std::vector<double> hw_states_positions_;
+  std::vector<double> hw_states_velocities_;
+  std::vector<double> hw_states_efforts_;
+
+  // Define expected joint order, so that it doesn't matter what order is defined in the URDF.
+  // It is expected that the joint name should contain these specifiers.
+  const std::vector<std::string> joint_order_;
+  std::vector<std::string> joints_names_sorted_;
+
+  std::shared_ptr<GPIOControllerInterface> gpio_controller_;
+  std::shared_ptr<RobotDriverInterface> robot_driver_;
+  std::shared_ptr<EStopInterface> e_stop_;
+
+  DrivetrainSettings drivetrain_settings_;
+  CANopenSettings canopen_settings_;
+
+  std::unique_ptr<SystemROSInterface> system_ros_interface_;
+
+  // Sometimes SDO errors can happen during initialization and activation of Roboteq drivers,
+  // in these cases it is better to retry
+  // Example errors:
+  // SDO abort code 05040000 received on upload request of object 1000 (Device type) to
+  // node 02: SDO protocol timed out
+  // SDO abort code 05040000 received on upload request of sub-object 1018:01 (Vendor-ID) to
+  // node 02: SDO protocol timed out
+  unsigned max_roboteq_initialization_attempts_;
+  unsigned max_roboteq_activation_attempts_;
+
+  rclcpp::Logger logger_{rclcpp::get_logger("UGVSystem")};
+  rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
+
+  std::shared_ptr<RoboteqErrorFilter> roboteq_error_filter_;
+
+  std::shared_ptr<std::mutex> robot_driver_write_mtx_;
+
+  rclcpp::Time next_driver_state_update_time_{0, 0, RCL_ROS_TIME};
+  rclcpp::Duration driver_states_update_period_{0, 0};
+};
+
+}  // namespace panther_hardware_interfaces
+
+#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_UGV_SYSTEM_HPP_
diff --git a/panther_hardware_interfaces/panther_hardware_interfaces.xml b/panther_hardware_interfaces/panther_hardware_interfaces.xml
index 8462de75..dbb3512e 100644
--- a/panther_hardware_interfaces/panther_hardware_interfaces.xml
+++ b/panther_hardware_interfaces/panther_hardware_interfaces.xml
@@ -7,6 +7,14 @@
     </description>
   </class>
 
+  <class name="panther_hardware_interfaces/LynxSystem"
+    type="panther_hardware_interfaces::LynxSystem"
+    base_class_type="hardware_interface::SystemInterface">
+    <description>
+      Hardware system controller for Panther's Roboteq motor controller
+    </description>
+  </class>
+
   <class name="panther_hardware_interfaces/PantherImuSensor"
     type="panther_hardware_interfaces::PantherImuSensor"
     base_class_type="hardware_interface::SensorInterface">
diff --git a/panther_hardware_interfaces/src/panther_system/lynx_system.cpp b/panther_hardware_interfaces/src/panther_system/lynx_system.cpp
new file mode 100644
index 00000000..8d491af5
--- /dev/null
+++ b/panther_hardware_interfaces/src/panther_system/lynx_system.cpp
@@ -0,0 +1,176 @@
+// 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_hardware_interfaces/panther_system/lynx_system.hpp"
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "diagnostic_updater/diagnostic_status_wrapper.hpp"
+#include "rclcpp/logging.hpp"
+
+#include "panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp"
+
+#include "panther_utils/diagnostics.hpp"
+
+namespace panther_hardware_interfaces
+{
+
+void LynxSystem::ReadCANopenSettingsDriverCANIDs()
+{
+  const auto driver_can_id = std::stoi(info_.hardware_parameters["driver_can_id"]);
+  canopen_settings_.driver_can_ids.emplace(DriverNames::DEFAULT, driver_can_id);
+}
+
+void LynxSystem::DefineRobotDriver()
+{
+  robot_driver_ = std::make_shared<LynxRobotDriver>(canopen_settings_, drivetrain_settings_);
+}
+
+void LynxSystem::UpdateHwStates()
+{
+  const auto data = robot_driver_->GetData(DriverNames::DEFAULT);
+
+  const auto left = data.GetMotorState(MotorChannels::LEFT);
+  const auto right = data.GetMotorState(MotorChannels::RIGHT);
+
+  hw_states_positions_[0] = left.GetPosition();
+  hw_states_positions_[1] = right.GetPosition();
+  hw_states_positions_[2] = left.GetPosition();
+  hw_states_positions_[3] = right.GetPosition();
+
+  hw_states_velocities_[0] = left.GetVelocity();
+  hw_states_velocities_[1] = right.GetVelocity();
+  hw_states_velocities_[2] = left.GetVelocity();
+  hw_states_velocities_[3] = right.GetVelocity();
+
+  hw_states_efforts_[0] = left.GetTorque();
+  hw_states_efforts_[1] = right.GetTorque();
+  hw_states_efforts_[2] = left.GetTorque();
+  hw_states_efforts_[3] = right.GetTorque();
+}
+
+void LynxSystem::UpdateMotorsStateDataTimedOut()
+{
+  if (robot_driver_->GetData(DriverNames::DEFAULT).IsMotorStatesDataTimedOut()) {
+    RCLCPP_WARN_STREAM_THROTTLE(logger_, steady_clock_, 1000, "PDO motor state data timeout.");
+    roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true);
+  } else {
+    roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, false);
+  }
+}
+
+void LynxSystem::UpdateDriverStateMsg()
+{
+  const auto driver_data = robot_driver_->GetData(DriverNames::DEFAULT);
+
+  system_ros_interface_->UpdateMsgDriversStates(DriverNames::DEFAULT, driver_data.GetDriverState());
+  system_ros_interface_->UpdateMsgErrorFlags(DriverNames::DEFAULT, driver_data);
+
+  CANErrors can_errors;
+  can_errors.error = roboteq_error_filter_->IsError();
+  can_errors.write_pdo_cmds_error = roboteq_error_filter_->IsError(ErrorsFilterIds::WRITE_PDO_CMDS);
+  can_errors.read_pdo_motor_states_error =
+    roboteq_error_filter_->IsError(ErrorsFilterIds::READ_PDO_MOTOR_STATES);
+  can_errors.read_pdo_driver_state_error =
+    roboteq_error_filter_->IsError(ErrorsFilterIds::READ_PDO_DRIVER_STATE);
+
+  DriverCANErrors driver_can_errors;
+  driver_can_errors.motor_states_data_timed_out = driver_data.IsMotorStatesDataTimedOut();
+  driver_can_errors.driver_state_data_timed_out = driver_data.IsDriverStateDataTimedOut();
+  driver_can_errors.can_error = driver_data.IsCANError();
+  driver_can_errors.heartbeat_timeout = driver_data.IsHeartbeatTimeout();
+
+  can_errors.driver_errors.emplace(DriverNames::DEFAULT, driver_can_errors);
+
+  system_ros_interface_->UpdateMsgErrors(can_errors);
+}
+
+void LynxSystem::UpdateFlagErrors()
+{
+  const auto driver_data = robot_driver_->GetData(DriverNames::DEFAULT);
+  if (driver_data.IsFlagError()) {
+    RCLCPP_WARN_STREAM_THROTTLE(
+      logger_, steady_clock_, 1000,
+      "Error state on the default driver: " << driver_data.GetFlagErrorLog());
+    roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, true);
+
+    HandleRobotDriverWriteOperation([this] { robot_driver_->AttemptErrorFlagReset(); });
+  } else {
+    roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false);
+  }
+}
+
+void LynxSystem::UpdateDriverStateDataTimedOut()
+{
+  if (robot_driver_->GetData(DriverNames::DEFAULT).IsDriverStateDataTimedOut()) {
+    RCLCPP_WARN_STREAM_THROTTLE(logger_, steady_clock_, 1000, "PDO driver state timeout.");
+    roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true);
+  } else {
+    roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, false);
+  }
+}
+
+void LynxSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status)
+{
+  unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK};
+  std::string message{"No error detected."};
+
+  const auto driver_data = robot_driver_->GetData(DriverNames::DEFAULT);
+  if (driver_data.IsError()) {
+    level = diagnostic_updater::DiagnosticStatusWrapper::ERROR;
+    message = "Error detected.";
+
+    panther_utils::diagnostics::AddKeyValueIfTrue(
+      status, driver_data.GetErrorMap(), "Driver error: ");
+  }
+
+  if (roboteq_error_filter_->IsError()) {
+    level = diagnostic_updater::DiagnosticStatusWrapper::ERROR;
+    message = "Error detected.";
+
+    panther_utils::diagnostics::AddKeyValueIfTrue(
+      status, roboteq_error_filter_->GetErrorMap(), "", " error");
+  }
+
+  status.summary(level, message);
+}
+
+void LynxSystem::DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status)
+{
+  unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK};
+  std::string message{"Panther system status monitoring."};
+
+  const auto driver_state = robot_driver_->GetData(DriverNames::DEFAULT).GetDriverState();
+
+  status.add("Default driver voltage (V)", driver_state.GetVoltage());
+  status.add("Default driver current (A)", driver_state.GetCurrent());
+  status.add("Default driver temperature (\u00B0C)", driver_state.GetTemperature());
+  status.add(
+    "Default driver heatsink temperature (\u00B0C)", driver_state.GetHeatsinkTemperature());
+
+  status.summary(level, message);
+}
+
+std::vector<float> LynxSystem::GetSpeedCommands() const
+{
+  return {
+    static_cast<float>(hw_commands_velocities_[0]), static_cast<float>(hw_commands_velocities_[1])};
+}
+
+}  // namespace panther_hardware_interfaces
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(panther_hardware_interfaces::LynxSystem, hardware_interface::SystemInterface)
diff --git a/panther_hardware_interfaces/src/panther_system/panther_system.cpp b/panther_hardware_interfaces/src/panther_system/panther_system.cpp
index a624c58a..e87ded9b 100644
--- a/panther_hardware_interfaces/src/panther_system/panther_system.cpp
+++ b/panther_hardware_interfaces/src/panther_system/panther_system.cpp
@@ -14,542 +14,65 @@
 
 #include "panther_hardware_interfaces/panther_system/panther_system.hpp"
 
-#include <array>
-#include <chrono>
-#include <functional>
-#include <limits>
 #include <memory>
-#include <stdexcept>
 #include <string>
 #include <vector>
 
 #include "diagnostic_updater/diagnostic_status_wrapper.hpp"
-#include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp/logging.hpp"
 
-#include "panther_hardware_interfaces/utils.hpp"
-#include "panther_utils/common_utilities.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp"
+
 #include "panther_utils/diagnostics.hpp"
 
 namespace panther_hardware_interfaces
 {
 
-CallbackReturn PantherSystem::on_init(const hardware_interface::HardwareInfo & hardware_info)
-{
-  if (hardware_interface::SystemInterface::on_init(hardware_info) != CallbackReturn::SUCCESS) {
-    return CallbackReturn::ERROR;
-  }
-
-  try {
-    CheckJointSize();
-    SortAndCheckJointNames();
-    SetInitialValues();
-    CheckInterfaces();
-  } catch (const std::runtime_error & e) {
-    RCLCPP_ERROR_STREAM(logger_, "An exception occurred while initializing: " << e.what());
-    return CallbackReturn::ERROR;
-  }
-
-  try {
-    ReadPantherVersion();
-    ReadDrivetrainSettings();
-    ReadCANopenSettings();
-    ReadInitializationActivationAttempts();
-    ReadParametersAndCreateRoboteqErrorFilter();
-    ReadDriverStatesUpdateFrequency();
-  } catch (const std::invalid_argument & e) {
-    RCLCPP_ERROR_STREAM(
-      logger_, "An exception occurred while reading the parameters: " << e.what());
-    return CallbackReturn::ERROR;
-  }
-
-  return CallbackReturn::SUCCESS;
-}
-
-CallbackReturn PantherSystem::on_configure(const rclcpp_lifecycle::State &)
-{
-  RCLCPP_DEBUG_STREAM(
-    logger_, "Creating system entities for the Panther version: " << panther_version_);
-
-  try {
-    ConfigureGPIOController();
-    ConfigureMotorsController();
-    ConfigureEStop();
-  } catch (const std::runtime_error & e) {
-    RCLCPP_ERROR_STREAM(
-      logger_, "Failed to initialize GPIO, Motors, or E-Stop controllers. Error: " << e.what());
-    return CallbackReturn::ERROR;
-  }
-
-  return CallbackReturn::SUCCESS;
-}
-
-CallbackReturn PantherSystem::on_cleanup(const rclcpp_lifecycle::State &)
-{
-  motors_controller_->Deinitialize();
-  motors_controller_.reset();
-
-  gpio_controller_.reset();
-  e_stop_.reset();
-
-  return CallbackReturn::SUCCESS;
-}
-
-CallbackReturn PantherSystem::on_activate(const rclcpp_lifecycle::State &)
-{
-  hw_commands_velocities_.fill(0.0);
-  hw_states_positions_.fill(0.0);
-  hw_states_velocities_.fill(0.0);
-  hw_states_efforts_.fill(0.0);
-
-  if (!OperationWithAttempts(
-        std::bind(&MotorsController::Activate, motors_controller_),
-        max_roboteq_activation_attempts_)) {
-    RCLCPP_ERROR_STREAM(
-      logger_, "Failed to activate Panther System: Couldn't activate MotorsController in "
-                 << max_roboteq_activation_attempts_ << " attempts.");
-    return CallbackReturn::ERROR;
-  }
-
-  panther_system_ros_interface_ =
-    std::make_unique<PantherSystemRosInterface>("hardware_controller");
-
-  panther_system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
-    "~/fan_enable",
-    std::bind(&GPIOControllerInterface::FanEnable, gpio_controller_, std::placeholders::_1));
-  panther_system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
-    "~/aux_power_enable",
-    std::bind(&GPIOControllerInterface::AUXPowerEnable, gpio_controller_, std::placeholders::_1));
-  panther_system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
-    "~/digital_power_enable",
-    std::bind(
-      &GPIOControllerInterface::DigitalPowerEnable, gpio_controller_, std::placeholders::_1));
-  panther_system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
-    "~/charger_enable",
-    std::bind(&GPIOControllerInterface::ChargerEnable, gpio_controller_, std::placeholders::_1));
-  panther_system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
-    "~/led_control_enable",
-    std::bind(&GPIOControllerInterface::LEDControlEnable, gpio_controller_, std::placeholders::_1));
-  panther_system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
-    "~/motor_power_enable",
-    std::bind(&PantherSystem::MotorsPowerEnable, this, std::placeholders::_1));
-
-  panther_system_ros_interface_->AddService<TriggerSrv, std::function<void()>>(
-    "~/e_stop_trigger", std::bind(&EStopInterface::TriggerEStop, e_stop_), 1,
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  auto e_stop_reset_qos = rmw_qos_profile_services_default;
-  e_stop_reset_qos.depth = 1;
-  panther_system_ros_interface_->AddService<TriggerSrv, std::function<void()>>(
-    "~/e_stop_reset", std::bind(&EStopInterface::ResetEStop, e_stop_), 2,
-    rclcpp::CallbackGroupType::MutuallyExclusive, e_stop_reset_qos);
-
-  panther_system_ros_interface_->AddDiagnosticTask(
-    std::string("system errors"), this, &PantherSystem::DiagnoseErrors);
-
-  panther_system_ros_interface_->AddDiagnosticTask(
-    std::string("system status"), this, &PantherSystem::DiagnoseStatus);
-
-  gpio_controller_->RegisterGPIOEventCallback(
-    [this](const auto & state) { panther_system_ros_interface_->PublishIOState(state); });
-
-  const auto io_state = gpio_controller_->QueryControlInterfaceIOStates();
-  panther_system_ros_interface_->InitializeAndPublishIOStateMsg(io_state);
-
-  panther_system_ros_interface_->PublishEStopStateMsg(e_stop_->ReadEStopState());
-
-  return CallbackReturn::SUCCESS;
-}
-
-CallbackReturn PantherSystem::on_deactivate(const rclcpp_lifecycle::State &)
-{
-  try {
-    e_stop_->TriggerEStop();
-  } catch (const std::runtime_error & e) {
-    RCLCPP_ERROR_STREAM(logger_, "Deactivation failed: " << e.what());
-    return CallbackReturn::ERROR;
-  }
-
-  panther_system_ros_interface_.reset();
-
-  return CallbackReturn::SUCCESS;
-}
-
-CallbackReturn PantherSystem::on_shutdown(const rclcpp_lifecycle::State &)
-{
-  try {
-    e_stop_->TriggerEStop();
-  } catch (const std::runtime_error & e) {
-    RCLCPP_ERROR_STREAM(logger_, "Shutdown failed: " << e.what());
-    return CallbackReturn::ERROR;
-  }
-
-  motors_controller_->Deinitialize();
-  motors_controller_.reset();
-
-  gpio_controller_.reset();
-
-  panther_system_ros_interface_.reset();
-  e_stop_.reset();
-
-  return CallbackReturn::SUCCESS;
-}
-
-CallbackReturn PantherSystem::on_error(const rclcpp_lifecycle::State &)
-{
-  try {
-    e_stop_->TriggerEStop();
-  } catch (const std::runtime_error & e) {
-    RCLCPP_ERROR_STREAM(logger_, "Handling error failed: " << e.what());
-    return CallbackReturn::ERROR;
-  }
-
-  panther_system_ros_interface_->BroadcastOnDiagnosticTasks(
-    diagnostic_msgs::msg::DiagnosticStatus::ERROR,
-    "An error has occurred during a node state transition.");
-
-  panther_system_ros_interface_.reset();
-
-  motors_controller_->Deinitialize();
-  motors_controller_.reset();
-
-  gpio_controller_.reset();
-  e_stop_.reset();
-
-  return CallbackReturn::SUCCESS;
-}
-
-std::vector<StateInterface> PantherSystem::export_state_interfaces()
-{
-  std::vector<StateInterface> state_interfaces;
-  for (std::size_t i = 0; i < kJointsSize; i++) {
-    state_interfaces.emplace_back(StateInterface(
-      joints_names_sorted_[i], hardware_interface::HW_IF_POSITION, &hw_states_positions_[i]));
-    state_interfaces.emplace_back(StateInterface(
-      joints_names_sorted_[i], hardware_interface::HW_IF_VELOCITY, &hw_states_velocities_[i]));
-    state_interfaces.emplace_back(StateInterface(
-      joints_names_sorted_[i], hardware_interface::HW_IF_EFFORT, &hw_states_efforts_[i]));
-  }
-
-  return state_interfaces;
-}
-
-std::vector<CommandInterface> PantherSystem::export_command_interfaces()
-{
-  std::vector<CommandInterface> command_interfaces;
-  for (std::size_t i = 0; i < kJointsSize; i++) {
-    command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      joints_names_sorted_[i], hardware_interface::HW_IF_VELOCITY, &hw_commands_velocities_[i]));
-  }
-
-  return command_interfaces;
-}
-
-return_type PantherSystem::read(const rclcpp::Time & time, const rclcpp::Duration & /* period */)
-{
-  UpdateMotorsState();
-
-  if (time >= next_driver_state_update_time_) {
-    UpdateDriverState();
-    UpdateDriverStateMsg();
-    panther_system_ros_interface_->PublishRobotDriverState();
-    next_driver_state_update_time_ = time + driver_states_update_period_;
-  }
-
-  UpdateEStopState();
-
-  return return_type::OK;
-}
-
-return_type PantherSystem::write(
-  const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
-{
-  const bool e_stop = e_stop_->ReadEStopState();
-
-  if (!e_stop) {
-    HandlePDOWriteOperation([this] {
-      motors_controller_->SendSpeedCommands(
-        hw_commands_velocities_[0], hw_commands_velocities_[1], hw_commands_velocities_[2],
-        hw_commands_velocities_[3]);
-    });
-  }
-
-  return return_type::OK;
-}
-
-void PantherSystem::CheckJointSize() const
-{
-  if (info_.joints.size() != kJointsSize) {
-    throw std::runtime_error(
-      "Wrong number of joints defined: " + std::to_string(info_.joints.size()) + ", " +
-      std::to_string(kJointsSize) + "expected.");
-  }
-}
-
-void PantherSystem::SortAndCheckJointNames()
-{
-  // Sort joints names - later hw_states and hw_commands are accessed by static indexes, so it
-  // is necessary to make sure that joints are in specific order and order of definitions in URDF
-  // doesn't matter
-  for (std::size_t i = 0; i < kJointsSize; i++) {
-    std::size_t match_count = 0;
-
-    for (std::size_t j = 0; j < kJointsSize; j++) {
-      if (CheckIfJointNameContainValidSequence(info_.joints[j].name, joint_order_[i])) {
-        joints_names_sorted_[i] = info_.joints[j].name;
-        ++match_count;
-      }
-    }
-
-    if (match_count != 1) {
-      throw std::runtime_error(
-        "There should be exactly one joint containing " + joint_order_[i] + ", " +
-        std::to_string(match_count) + " found.");
-    }
-  }
-}
-
-void PantherSystem::SetInitialValues()
-{
-  // It isn't safe to set command to NaN - sometimes it could be interpreted as Inf (although it
-  // shouldn't). In case of velocity, I think that setting the initial value to 0.0 is the best
-  // option.
-  hw_commands_velocities_.fill(0.0);
-
-  hw_states_positions_.fill(std::numeric_limits<double>::quiet_NaN());
-  hw_states_velocities_.fill(std::numeric_limits<double>::quiet_NaN());
-  hw_states_efforts_.fill(std::numeric_limits<double>::quiet_NaN());
-}
-
-void PantherSystem::CheckInterfaces() const
-{
-  for (const hardware_interface::ComponentInfo & joint : info_.joints) {
-    // Commands
-    if (joint.command_interfaces.size() != 1) {
-      throw std::runtime_error(
-        "Joint " + joint.name + " has " + std::to_string(joint.command_interfaces.size()) +
-        " command interfaces. 1 expected.");
-    }
-
-    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) {
-      throw std::runtime_error(
-        "Joint " + joint.name + " has " + joint.command_interfaces[0].name +
-        " command interface. " + hardware_interface::HW_IF_VELOCITY + " expected.");
-    }
-
-    // States
-    if (joint.state_interfaces.size() != 3) {
-      throw std::runtime_error(
-        "Joint " + joint.name + " has " + std::to_string(joint.state_interfaces.size()) +
-        " state  " + (joint.state_interfaces.size() == 1 ? "interface." : "interfaces.") +
-        " 3 expected.");
-    }
-
-    if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
-      throw std::runtime_error(
-        "Joint " + joint.name + " has " + joint.state_interfaces[0].name +
-        " as first state interface. " + hardware_interface::HW_IF_POSITION + " expected.");
-    }
-
-    if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) {
-      throw std::runtime_error(
-        "Joint " + joint.name + " has " + joint.state_interfaces[1].name +
-        " as second state interface. " + hardware_interface::HW_IF_VELOCITY + " expected.");
-    }
-
-    if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) {
-      throw std::runtime_error(
-        "Joint " + joint.name + " has " + joint.state_interfaces[2].name +
-        " as third state interface. " + hardware_interface::HW_IF_EFFORT + " expected.");
-    }
-  }
-}
-
-void PantherSystem::ReadPantherVersion()
-{
-  panther_version_ = std::stof(info_.hardware_parameters["panther_version"]);
-}
-
-void PantherSystem::ReadDrivetrainSettings()
-{
-  drivetrain_settings_.motor_torque_constant =
-    std::stof(info_.hardware_parameters["motor_torque_constant"]);
-  drivetrain_settings_.gear_ratio = std::stof(info_.hardware_parameters["gear_ratio"]);
-  drivetrain_settings_.gearbox_efficiency =
-    std::stof(info_.hardware_parameters["gearbox_efficiency"]);
-  drivetrain_settings_.encoder_resolution =
-    std::stof(info_.hardware_parameters["encoder_resolution"]);
-  drivetrain_settings_.max_rpm_motor_speed =
-    std::stof(info_.hardware_parameters["max_rpm_motor_speed"]);
-}
-
-void PantherSystem::ReadCANopenSettings()
-{
-  canopen_settings_.can_interface_name = info_.hardware_parameters["can_interface_name"];
-  canopen_settings_.master_can_id = std::stoi(info_.hardware_parameters["master_can_id"]);
-  canopen_settings_.front_driver_can_id =
-    std::stoi(info_.hardware_parameters["front_driver_can_id"]);
-  canopen_settings_.rear_driver_can_id = std::stoi(info_.hardware_parameters["rear_driver_can_id"]);
-  canopen_settings_.pdo_motor_states_timeout_ms =
-    std::chrono::milliseconds(std::stoi(info_.hardware_parameters["pdo_motor_states_timeout_ms"]));
-  canopen_settings_.pdo_driver_state_timeout_ms =
-    std::chrono::milliseconds(std::stoi(info_.hardware_parameters["pdo_driver_state_timeout_ms"]));
-  canopen_settings_.sdo_operation_timeout_ms =
-    std::chrono::milliseconds(std::stoi(info_.hardware_parameters["sdo_operation_timeout_ms"]));
-}
-
-void PantherSystem::ReadInitializationActivationAttempts()
+void PantherSystem::ReadCANopenSettingsDriverCANIDs()
 {
-  max_roboteq_initialization_attempts_ =
-    std::stoi(info_.hardware_parameters["max_roboteq_initialization_attempts"]);
-  max_roboteq_activation_attempts_ =
-    std::stoi(info_.hardware_parameters["max_roboteq_activation_attempts"]);
-}
-
-void PantherSystem::ReadParametersAndCreateRoboteqErrorFilter()
-{
-  const unsigned max_write_pdo_cmds_errors_count =
-    std::stoi(info_.hardware_parameters["max_write_pdo_cmds_errors_count"]);
-  const unsigned max_read_pdo_motor_states_errors_count =
-    std::stoi(info_.hardware_parameters["max_read_pdo_motor_states_errors_count"]);
-  const unsigned max_read_pdo_driver_state_errors_count =
-    std::stoi(info_.hardware_parameters["max_read_pdo_driver_state_errors_count"]);
-
-  roboteq_error_filter_ = std::make_shared<RoboteqErrorFilter>(
-    max_write_pdo_cmds_errors_count, max_read_pdo_motor_states_errors_count,
-    max_read_pdo_driver_state_errors_count, 1);
-}
-
-void PantherSystem::ReadDriverStatesUpdateFrequency()
-{
-  const float driver_states_update_frequency =
-    std::stof(info_.hardware_parameters["driver_states_update_frequency"]);
-  driver_states_update_period_ =
-    rclcpp::Duration::from_seconds(1.0f / driver_states_update_frequency);
-}
-
-void PantherSystem::ConfigureGPIOController()
-{
-  gpio_controller_ = GPIOControllerFactory::CreateGPIOController(panther_version_);
-  gpio_controller_->Start();
-
-  RCLCPP_INFO(logger_, "Successfully configured GPIO controller.");
-}
-
-void PantherSystem::ConfigureMotorsController()
-{
-  motor_controller_write_mtx_ = std::make_shared<std::mutex>();
-  motors_controller_ = std::make_shared<MotorsController>(canopen_settings_, drivetrain_settings_);
-
-  if (!OperationWithAttempts(
-        std::bind(&MotorsController::Initialize, motors_controller_),
-        max_roboteq_initialization_attempts_,
-        std::bind(&MotorsController::Deinitialize, motors_controller_))) {
-    throw std::runtime_error("Roboteq drivers initialization failed.");
-  }
-
-  RCLCPP_INFO(logger_, "Successfully configured motors controller");
-}
-
-void PantherSystem::ConfigureEStop()
-{
-  if (
-    !gpio_controller_ || !roboteq_error_filter_ || !motors_controller_ ||
-    !motor_controller_write_mtx_) {
-    throw std::runtime_error("Failed to configure E-Stop, make sure to setup entities first.");
-  }
-
-  if (panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2)) {
-    e_stop_ = std::make_shared<EStopPTH12X>(
-      gpio_controller_, roboteq_error_filter_, motors_controller_, motor_controller_write_mtx_,
-      std::bind(&PantherSystem::AreVelocityCommandsNearZero, this));
-  } else {
-    e_stop_ = std::make_shared<EStopPTH10X>(
-      gpio_controller_, roboteq_error_filter_, motors_controller_, motor_controller_write_mtx_,
-      std::bind(&PantherSystem::AreVelocityCommandsNearZero, this));
-  }
-
-  RCLCPP_INFO(logger_, "Successfully configured E-Stop");
+  const auto front_driver_can_id = std::stoi(info_.hardware_parameters["front_driver_can_id"]);
+  const auto rear_driver_can_id = std::stoi(info_.hardware_parameters["rear_driver_can_id"]);
+  canopen_settings_.driver_can_ids.emplace(DriverNames::FRONT, front_driver_can_id);
+  canopen_settings_.driver_can_ids.emplace(DriverNames::REAR, rear_driver_can_id);
 }
 
-void PantherSystem::UpdateMotorsState()
+void PantherSystem::DefineRobotDriver()
 {
-  try {
-    motors_controller_->UpdateMotorsState();
-    UpdateHwStates();
-    UpdateMotorsStateDataTimedOut();
-  } catch (const std::runtime_error & e) {
-    roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true);
-
-    RCLCPP_ERROR_STREAM_THROTTLE(
-      logger_, steady_clock_, 5000,
-      "An exception occurred while updating motors states: " << e.what());
-  }
+  robot_driver_ = std::make_shared<PantherRobotDriver>(canopen_settings_, drivetrain_settings_);
 }
 
-void PantherSystem::UpdateDriverState()
+void PantherSystem::UpdateHwStates()
 {
-  try {
-    motors_controller_->UpdateDriversState();
-    UpdateFlagErrors();
-    UpdateDriverStateDataTimedOut();
-  } catch (const std::runtime_error & e) {
-    roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true);
+  const auto front_data = robot_driver_->GetData(DriverNames::FRONT);
+  const auto rear_data = robot_driver_->GetData(DriverNames::REAR);
 
-    RCLCPP_ERROR_STREAM_THROTTLE(
-      logger_, steady_clock_, 5000,
-      "An exception occurred while updating drivers states: " << e.what());
-  }
-}
+  const auto fl_motor_state = front_data.GetMotorState(MotorChannels::LEFT);
+  const auto fr_motor_state = front_data.GetMotorState(MotorChannels::RIGHT);
+  const auto rl_motor_state = rear_data.GetMotorState(MotorChannels::LEFT);
+  const auto rr_motor_state = rear_data.GetMotorState(MotorChannels::RIGHT);
 
-void PantherSystem::UpdateEStopState()
-{
-  if (
-    motors_controller_->GetFrontData().IsHeartbeatTimeout() ||
-    motors_controller_->GetRearData().IsHeartbeatTimeout()) {
-    e_stop_->TriggerEStop();
-  }
+  hw_states_positions_[0] = fl_motor_state.GetPosition();
+  hw_states_positions_[1] = fr_motor_state.GetPosition();
+  hw_states_positions_[2] = rl_motor_state.GetPosition();
+  hw_states_positions_[3] = rr_motor_state.GetPosition();
 
-  const bool e_stop = e_stop_->ReadEStopState();
-  panther_system_ros_interface_->PublishEStopStateIfChanged(e_stop);
-}
+  hw_states_velocities_[0] = fl_motor_state.GetVelocity();
+  hw_states_velocities_[1] = fr_motor_state.GetVelocity();
+  hw_states_velocities_[2] = rl_motor_state.GetVelocity();
+  hw_states_velocities_[3] = rr_motor_state.GetVelocity();
 
-void PantherSystem::UpdateHwStates()
-{
-  const auto front = motors_controller_->GetFrontData();
-  const auto rear = motors_controller_->GetRearData();
-
-  const auto fl = front.GetLeftMotorState();
-  const auto fr = front.GetRightMotorState();
-  const auto rl = rear.GetLeftMotorState();
-  const auto rr = rear.GetRightMotorState();
-
-  hw_states_positions_[0] = fl.GetPosition();
-  hw_states_positions_[1] = fr.GetPosition();
-  hw_states_positions_[2] = rl.GetPosition();
-  hw_states_positions_[3] = rr.GetPosition();
-
-  hw_states_velocities_[0] = fl.GetVelocity();
-  hw_states_velocities_[1] = fr.GetVelocity();
-  hw_states_velocities_[2] = rl.GetVelocity();
-  hw_states_velocities_[3] = rr.GetVelocity();
-
-  hw_states_efforts_[0] = fl.GetTorque();
-  hw_states_efforts_[1] = fr.GetTorque();
-  hw_states_efforts_[2] = rl.GetTorque();
-  hw_states_efforts_[3] = rr.GetTorque();
+  hw_states_efforts_[0] = fl_motor_state.GetTorque();
+  hw_states_efforts_[1] = fr_motor_state.GetTorque();
+  hw_states_efforts_[2] = rl_motor_state.GetTorque();
+  hw_states_efforts_[3] = rr_motor_state.GetTorque();
 }
 
 void PantherSystem::UpdateMotorsStateDataTimedOut()
 {
   if (
-    motors_controller_->GetFrontData().IsMotorStatesDataTimedOut() ||
-    motors_controller_->GetRearData().IsMotorStatesDataTimedOut()) {
-    RCLCPP_WARN_STREAM_THROTTLE(
-      logger_, steady_clock_, 1000,
-      (motors_controller_->GetFrontData().IsMotorStatesDataTimedOut() ? "Front " : "")
-        << (motors_controller_->GetRearData().IsMotorStatesDataTimedOut() ? "Rear " : "")
-        << "PDO motor state data timeout.");
+    robot_driver_->GetData(DriverNames::FRONT).IsMotorStatesDataTimedOut() ||
+    robot_driver_->GetData(DriverNames::REAR).IsMotorStatesDataTimedOut()) {
+    RCLCPP_WARN_STREAM_THROTTLE(logger_, steady_clock_, 1000, "PDO motor state data timeout.");
     roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true);
   } else {
     roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, false);
@@ -558,12 +81,15 @@ void PantherSystem::UpdateMotorsStateDataTimedOut()
 
 void PantherSystem::UpdateDriverStateMsg()
 {
-  panther_system_ros_interface_->UpdateMsgDriversStates(
-    motors_controller_->GetFrontData().GetDriverState(),
-    motors_controller_->GetRearData().GetDriverState());
+  const auto front_driver_data = robot_driver_->GetData(DriverNames::FRONT);
+  const auto rear_driver_data = robot_driver_->GetData(DriverNames::REAR);
 
-  panther_system_ros_interface_->UpdateMsgErrorFlags(
-    motors_controller_->GetFrontData(), motors_controller_->GetRearData());
+  system_ros_interface_->UpdateMsgDriversStates(
+    DriverNames::FRONT, front_driver_data.GetDriverState());
+  system_ros_interface_->UpdateMsgDriversStates(
+    DriverNames::REAR, rear_driver_data.GetDriverState());
+  system_ros_interface_->UpdateMsgErrorFlags(DriverNames::FRONT, front_driver_data);
+  system_ros_interface_->UpdateMsgErrorFlags(DriverNames::REAR, rear_driver_data);
 
   CANErrors can_errors;
   can_errors.error = roboteq_error_filter_->IsError();
@@ -573,38 +99,40 @@ void PantherSystem::UpdateDriverStateMsg()
   can_errors.read_pdo_driver_state_error =
     roboteq_error_filter_->IsError(ErrorsFilterIds::READ_PDO_DRIVER_STATE);
 
-  can_errors.front_motor_states_data_timed_out =
-    motors_controller_->GetFrontData().IsMotorStatesDataTimedOut();
-  can_errors.rear_motor_states_data_timed_out =
-    motors_controller_->GetRearData().IsMotorStatesDataTimedOut();
+  DriverCANErrors front_driver_can_errors;
+  DriverCANErrors rear_driver_can_errors;
 
-  can_errors.front_driver_state_data_timed_out =
-    motors_controller_->GetFrontData().IsDriverStateDataTimedOut();
-  can_errors.rear_driver_state_data_timed_out =
-    motors_controller_->GetRearData().IsDriverStateDataTimedOut();
+  front_driver_can_errors.motor_states_data_timed_out =
+    front_driver_data.IsMotorStatesDataTimedOut();
+  front_driver_can_errors.driver_state_data_timed_out =
+    front_driver_data.IsDriverStateDataTimedOut();
+  front_driver_can_errors.can_error = front_driver_data.IsCANError();
+  front_driver_can_errors.heartbeat_timeout = front_driver_data.IsHeartbeatTimeout();
 
-  can_errors.front_can_error = motors_controller_->GetFrontData().IsCANError();
-  can_errors.rear_can_error = motors_controller_->GetRearData().IsCANError();
+  rear_driver_can_errors.motor_states_data_timed_out = rear_driver_data.IsMotorStatesDataTimedOut();
+  rear_driver_can_errors.driver_state_data_timed_out = rear_driver_data.IsDriverStateDataTimedOut();
+  rear_driver_can_errors.can_error = rear_driver_data.IsCANError();
+  rear_driver_can_errors.heartbeat_timeout = rear_driver_data.IsHeartbeatTimeout();
 
-  can_errors.front_heartbeat_timeout = motors_controller_->GetFrontData().IsHeartbeatTimeout();
-  can_errors.rear_heartbeat_timeout = motors_controller_->GetRearData().IsHeartbeatTimeout();
+  can_errors.driver_errors.emplace(DriverNames::FRONT, front_driver_can_errors);
+  can_errors.driver_errors.emplace(DriverNames::REAR, rear_driver_can_errors);
 
-  panther_system_ros_interface_->UpdateMsgErrors(can_errors);
+  system_ros_interface_->UpdateMsgErrors(can_errors);
 }
 
 void PantherSystem::UpdateFlagErrors()
 {
-  if (
-    motors_controller_->GetFrontData().IsFlagError() ||
-    motors_controller_->GetRearData().IsFlagError()) {
+  const auto front_driver_data = robot_driver_->GetData(DriverNames::FRONT);
+  const auto rear_driver_data = robot_driver_->GetData(DriverNames::REAR);
+  if (front_driver_data.IsFlagError() || rear_driver_data.IsFlagError()) {
     RCLCPP_WARN_STREAM_THROTTLE(
       logger_, steady_clock_, 1000,
       "Error state on one of the drivers:\n"
-        << "\tFront: " << motors_controller_->GetFrontData().GetFlagErrorLog()
-        << "\tRear: " << motors_controller_->GetRearData().GetFlagErrorLog());
+        << "\tFront: " << front_driver_data.GetFlagErrorLog()
+        << "\tRear: " << rear_driver_data.GetFlagErrorLog());
     roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, true);
 
-    HandlePDOWriteOperation([this] { motors_controller_->AttemptErrorFlagResetWithZeroSpeed(); });
+    HandleRobotDriverWriteOperation([this] { robot_driver_->AttemptErrorFlagReset(); });
   } else {
     roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false);
   }
@@ -613,77 +141,21 @@ void PantherSystem::UpdateFlagErrors()
 void PantherSystem::UpdateDriverStateDataTimedOut()
 {
   if (
-    motors_controller_->GetFrontData().IsDriverStateDataTimedOut() ||
-    motors_controller_->GetRearData().IsDriverStateDataTimedOut()) {
-    RCLCPP_WARN_STREAM_THROTTLE(
-      logger_, steady_clock_, 1000,
-      (motors_controller_->GetFrontData().IsDriverStateDataTimedOut() ? "Front " : "")
-        << (motors_controller_->GetRearData().IsDriverStateDataTimedOut() ? "Rear " : "")
-        << "PDO driver state timeout.");
+    robot_driver_->GetData(DriverNames::FRONT).IsDriverStateDataTimedOut() ||
+    robot_driver_->GetData(DriverNames::REAR).IsDriverStateDataTimedOut()) {
+    RCLCPP_WARN_STREAM_THROTTLE(logger_, steady_clock_, 1000, "PDO driver state timeout.");
     roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true);
   } else {
     roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, false);
   }
 }
 
-void PantherSystem::HandlePDOWriteOperation(std::function<void()> pdo_write_operation)
-{
-  try {
-    {
-      std::unique_lock<std::mutex> motor_controller_write_lck(
-        *motor_controller_write_mtx_, std::defer_lock);
-      if (!motor_controller_write_lck.try_lock()) {
-        throw std::runtime_error(
-          "Can't acquire mutex for writing commands - E-stop is being triggered.");
-      }
-      pdo_write_operation();
-    }
-
-    roboteq_error_filter_->UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, false);
-  } catch (const std::runtime_error & e) {
-    RCLCPP_WARN_STREAM(logger_, "An exception occurred while writing commands: " << e.what());
-    roboteq_error_filter_->UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true);
-  }
-}
-
-bool PantherSystem::AreVelocityCommandsNearZero()
-{
-  for (const auto & cmd : hw_commands_velocities_) {
-    if (std::abs(cmd) > std::numeric_limits<double>::epsilon()) {
-      return false;
-    }
-  }
-  return true;
-}
-
-void PantherSystem::MotorsPowerEnable(const bool enable)
-{
-  try {
-    {
-      std::lock_guard<std::mutex> lck_g(*motor_controller_write_mtx_);
-
-      if (!enable) {
-        motors_controller_->TurnOnEStop();
-      } else {
-        motors_controller_->TurnOffEStop();
-      }
-    }
-
-    e_stop_->TriggerEStop();
-
-    roboteq_error_filter_->SetClearErrorsFlag();
-    roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false);
-  } catch (const std::runtime_error & e) {
-    RCLCPP_WARN_STREAM(logger_, "An exception occurred while enabling motors power: " << e.what());
-  }
-}
-
 void PantherSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status)
 {
   unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK};
   std::string message{"No error detected."};
 
-  const auto front_driver_data = motors_controller_->GetFrontData();
+  const auto front_driver_data = robot_driver_->GetData(DriverNames::FRONT);
   if (front_driver_data.IsError()) {
     level = diagnostic_updater::DiagnosticStatusWrapper::ERROR;
     message = "Error detected.";
@@ -692,7 +164,7 @@ void PantherSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper &
       status, front_driver_data.GetErrorMap(), "Front driver error: ");
   }
 
-  const auto rear_driver_data = motors_controller_->GetRearData();
+  const auto rear_driver_data = robot_driver_->GetData(DriverNames::REAR);
   if (rear_driver_data.IsError()) {
     level = diagnostic_updater::DiagnosticStatusWrapper::ERROR;
     message = "Error detected.";
@@ -717,8 +189,8 @@ void PantherSystem::DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper &
   unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK};
   std::string message{"Panther system status monitoring."};
 
-  const auto front_driver_state = motors_controller_->GetFrontData().GetDriverState();
-  const auto rear_driver_state = motors_controller_->GetRearData().GetDriverState();
+  const auto front_driver_state = robot_driver_->GetData(DriverNames::FRONT).GetDriverState();
+  const auto rear_driver_state = robot_driver_->GetData(DriverNames::REAR).GetDriverState();
 
   auto driver_states_with_names = {
     std::make_pair(std::string("Front"), front_driver_state),
@@ -732,10 +204,16 @@ void PantherSystem::DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper &
       driver_name + " driver heatsink temperature (\u00B0C)",
       driver_state.GetHeatsinkTemperature());
   }
-
   status.summary(level, message);
 }
 
+std::vector<float> PantherSystem::GetSpeedCommands() const
+{
+  return {
+    static_cast<float>(hw_commands_velocities_[0]), static_cast<float>(hw_commands_velocities_[1]),
+    static_cast<float>(hw_commands_velocities_[2]), static_cast<float>(hw_commands_velocities_[3])};
+}
+
 }  // namespace panther_hardware_interfaces
 
 #include "pluginlib/class_list_macros.hpp"
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
index 81026556..9a3626c0 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
+++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
@@ -56,8 +56,11 @@ void RoboteqRobotDriver::Initialize()
     DefineDrivers();
     for (auto & [name, driver] : drivers_) {
       data_.emplace(name, DriverData(drivetrain_settings_));
-      driver->Boot();
     }
+
+    canopen_manager_.Activate();
+    BootDrivers();
+
   } catch (const std::runtime_error & e) {
     throw std::runtime_error("Failed to initialize robot driver: " + std::string(e.what()));
   }
@@ -81,7 +84,7 @@ void RoboteqRobotDriver::Activate()
       driver->ResetScript();
     } catch (const std::runtime_error & e) {
       throw std::runtime_error(
-        "Reset Roboteq script exception on " + name + "driver : " + std::string(e.what()));
+        "Reset Roboteq script exception on " + name + " driver : " + std::string(e.what()));
     }
   }
 
@@ -190,6 +193,13 @@ void RoboteqRobotDriver::TurnOffEStop()
   }
 }
 
+bool RoboteqRobotDriver::CommunicationError()
+{
+  return std::any_of(data_.begin(), data_.end(), [](const auto & data) {
+    return data.second.IsCANError() || data.second.IsHeartbeatTimeout();
+  });
+}
+
 void RoboteqRobotDriver::SetMotorsStates(
   DriverData & data, const MotorDriverState & left_state, const MotorDriverState & right_state,
   const timespec & current_time)
@@ -222,4 +232,29 @@ bool RoboteqRobotDriver::DataTimeout(
          timeout;
 }
 
+void RoboteqRobotDriver::BootDrivers()
+{
+  for (auto & [name, driver] : drivers_) {
+    try {
+      auto driver_future = driver->Boot();
+      auto driver_status = driver_future.wait_for(std::chrono::seconds(5));
+
+      if (driver_status == std::future_status::ready) {
+        try {
+          driver_future.get();
+        } catch (const std::exception & e) {
+          throw std::runtime_error(
+            "Boot for " + name + " driver failed with exception: " + std::string(e.what()));
+        }
+      } else {
+        throw std::runtime_error("Boot for " + name + " driver timed out or failed.");
+      }
+
+    } catch (const std::system_error & e) {
+      throw std::runtime_error(
+        "An exception occurred while trying to Boot " + name + " driver " + std::string(e.what()));
+    }
+  }
+}
+
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/panther_system_e_stop.cpp b/panther_hardware_interfaces/src/panther_system/system_e_stop.cpp
similarity index 57%
rename from panther_hardware_interfaces/src/panther_system/panther_system_e_stop.cpp
rename to panther_hardware_interfaces/src/panther_system/system_e_stop.cpp
index 9a776b40..35efc987 100644
--- a/panther_hardware_interfaces/src/panther_system/panther_system_e_stop.cpp
+++ b/panther_hardware_interfaces/src/panther_system/system_e_stop.cpp
@@ -17,12 +17,12 @@
 #include <memory>
 #include <string>
 
-#include "panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp"
+#include "panther_hardware_interfaces/panther_system/system_e_stop.hpp"
 
 namespace panther_hardware_interfaces
 {
 
-bool EStopPTH12X::ReadEStopState()
+bool EStop::ReadEStopState()
 {
   if (e_stop_manipulation_mtx_.try_lock()) {
     std::lock_guard<std::mutex> e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock);
@@ -39,7 +39,7 @@ bool EStopPTH12X::ReadEStopState()
   return e_stop_triggered_;
 }
 
-void EStopPTH12X::TriggerEStop()
+void EStop::TriggerEStop()
 {
   gpio_controller_->InterruptEStopReset();
 
@@ -53,7 +53,7 @@ void EStopPTH12X::TriggerEStop()
   e_stop_triggered_ = true;
 }
 
-void EStopPTH12X::ResetEStop()
+void EStop::ResetEStop()
 {
   if (e_stop_manipulation_mtx_.try_lock()) {
     std::lock_guard<std::mutex> e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock);
@@ -83,61 +83,4 @@ void EStopPTH12X::ResetEStop()
   }
 }
 
-bool EStopPTH10X::ReadEStopState()
-{
-  if (e_stop_manipulation_mtx_.try_lock()) {
-    std::lock_guard<std::mutex> e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock);
-    const bool motors_on = gpio_controller_->IsPinActive(GPIOPin::STAGE2_INPUT);
-    const bool driver_error = roboteq_error_filter_->IsError();
-
-    if ((driver_error || !motors_on) && !e_stop_triggered_) {
-      TriggerEStop();
-    }
-  }
-
-  return e_stop_triggered_;
-}
-
-void EStopPTH10X::TriggerEStop()
-{
-  std::lock_guard<std::mutex> e_stop_lck(e_stop_manipulation_mtx_);
-  std::lock_guard<std::mutex> lck_g(*motor_controller_write_mtx_);
-
-  try {
-    motors_controller_->TurnOnSafetyStop();
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error(
-      "Error when trying to set safety stop using CAN command: " + std::string(e.what()));
-  }
-
-  e_stop_triggered_ = true;
-}
-
-void EStopPTH10X::ResetEStop()
-{
-  if (e_stop_manipulation_mtx_.try_lock()) {
-    std::lock_guard<std::mutex> e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock);
-
-    if (!ZeroVelocityCheck()) {
-      throw std::runtime_error(
-        "Can't reset E-stop - last velocity commands are different than zero. Make sure that your "
-        "controller sends zero commands before trying to reset E-stop.");
-    }
-
-    if (!gpio_controller_->IsPinActive(GPIOPin::STAGE2_INPUT)) {
-      throw std::runtime_error("Can't reset E-stop - motors are not powered.");
-    }
-
-    if (roboteq_error_filter_->IsError()) {
-      throw std::runtime_error("Can't reset E-stop - motor controller is in an error state.");
-    }
-
-    roboteq_error_filter_->SetClearErrorsFlag();
-
-    e_stop_triggered_ = false;
-  } else {
-    std::fprintf(stdout, "E-stop trigger operation is in progress, skipping reset.");
-  }
-}
-
 }  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp b/panther_hardware_interfaces/src/panther_system/system_ros_interface.cpp
similarity index 61%
rename from panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp
rename to panther_hardware_interfaces/src/panther_system/system_ros_interface.cpp
index e6650784..f6b5564e 100644
--- a/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp
+++ b/panther_hardware_interfaces/src/panther_system/system_ros_interface.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp"
+#include "panther_hardware_interfaces/panther_system/system_ros_interface.hpp"
 
 #include <memory>
 #include <string>
@@ -22,7 +22,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "realtime_tools/realtime_publisher.h"
 
-#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
 
 namespace panther_hardware_interfaces
 {
@@ -52,7 +52,7 @@ void ROSServiceWrapper<SrvT, CallbackT>::CallbackWrapper(
     response->message = err.what();
 
     RCLCPP_WARN_STREAM(
-      rclcpp::get_logger("PantherSystem"),
+      rclcpp::get_logger("UGVSystem"),
       "An exception occurred while handling the request: " << err.what());
   }
 }
@@ -71,11 +71,11 @@ void ROSServiceWrapper<std_srvs::srv::Trigger, std::function<void()>>::ProccessC
   callback_();
 }
 
-PantherSystemRosInterface::PantherSystemRosInterface(
+SystemROSInterface::SystemROSInterface(
   const std::string & node_name, const rclcpp::NodeOptions & node_options)
 : node_(rclcpp::Node::make_shared(node_name, node_options)), diagnostic_updater_(node_)
 {
-  RCLCPP_INFO(rclcpp::get_logger("PantherSystem"), "Constructing node.");
+  RCLCPP_INFO(rclcpp::get_logger("UGVSystem"), "Constructing node.");
 
   executor_ = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();
   executor_->add_node(node_);
@@ -87,8 +87,6 @@ PantherSystemRosInterface::PantherSystemRosInterface(
     std::make_unique<realtime_tools::RealtimePublisher<RobotDriverStateMsg>>(
       driver_state_publisher_);
 
-  InitializeRobotDriverStateMsg();
-
   io_state_publisher_ = node_->create_publisher<IOStateMsg>(
     "~/io_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
   realtime_io_state_publisher_ =
@@ -99,12 +97,12 @@ PantherSystemRosInterface::PantherSystemRosInterface(
   realtime_e_stop_state_publisher_ =
     std::make_unique<realtime_tools::RealtimePublisher<BoolMsg>>(e_stop_state_publisher_);
 
-  diagnostic_updater_.setHardwareID("Panther System");
+  diagnostic_updater_.setHardwareID("UGV System");
 
-  RCLCPP_INFO(rclcpp::get_logger("PantherSystem"), "Node constructed successfully.");
+  RCLCPP_INFO(rclcpp::get_logger("UGVSystem"), "Node constructed successfully.");
 }
 
-PantherSystemRosInterface::~PantherSystemRosInterface()
+SystemROSInterface::~SystemROSInterface()
 {
   if (executor_) {
     executor_->cancel();
@@ -128,71 +126,55 @@ PantherSystemRosInterface::~PantherSystemRosInterface()
   node_.reset();
 }
 
-void PantherSystemRosInterface::UpdateMsgErrorFlags(
-  const RoboteqData & front, const RoboteqData & rear)
+void SystemROSInterface::UpdateMsgErrorFlags(const std::string & name, const DriverData & data)
 {
   auto & driver_state = realtime_driver_state_publisher_->msg_;
-  auto & front_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_FRONT);
-  auto & rear_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_REAR);
+  auto & driver_state_named = GetDriverStateByName(driver_state, name);
 
   driver_state.header.stamp = node_->get_clock()->now();
 
-  front_driver_state.state.fault_flag = front.GetFaultFlag().GetMessage();
-  front_driver_state.state.script_flag = front.GetScriptFlag().GetMessage();
-  front_driver_state.state.channel_2_motor_runtime_error = front.GetLeftRuntimeError().GetMessage();
-  front_driver_state.state.channel_1_motor_runtime_error = front.GetRightRuntimeError().GetMessage();
-
-  rear_driver_state.state.fault_flag = rear.GetFaultFlag().GetMessage();
-  rear_driver_state.state.script_flag = rear.GetScriptFlag().GetMessage();
-  rear_driver_state.state.channel_2_motor_runtime_error = rear.GetLeftRuntimeError().GetMessage();
-  rear_driver_state.state.channel_1_motor_runtime_error = rear.GetRightRuntimeError().GetMessage();
+  driver_state_named.state.fault_flag = data.GetFaultFlag().GetMessage();
+  driver_state_named.state.script_flag = data.GetScriptFlag().GetMessage();
+  driver_state_named.state.channel_1_motor_runtime_error =
+    data.GetRuntimeError(RoboteqDriver::kChannel1).GetMessage();
+  driver_state_named.state.channel_2_motor_runtime_error =
+    data.GetRuntimeError(RoboteqDriver::kChannel2).GetMessage();
 }
 
-void PantherSystemRosInterface::UpdateMsgDriversStates(
-  const DriverState & front, const DriverState & rear)
+void SystemROSInterface::UpdateMsgDriversStates(
+  const std::string & name, const RoboteqDriverState & state)
 {
   auto & driver_state = realtime_driver_state_publisher_->msg_;
-  auto & front_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_FRONT);
-  auto & rear_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_REAR);
-
-  front_driver_state.state.voltage = front.GetVoltage();
-  front_driver_state.state.current = front.GetCurrent();
-  front_driver_state.state.temperature = front.GetTemperature();
-  front_driver_state.state.heatsink_temperature = front.GetHeatsinkTemperature();
-
-  rear_driver_state.state.voltage = rear.GetVoltage();
-  rear_driver_state.state.current = rear.GetCurrent();
-  rear_driver_state.state.temperature = rear.GetTemperature();
-  rear_driver_state.state.heatsink_temperature = rear.GetHeatsinkTemperature();
+  auto & driver_state_named = GetDriverStateByName(driver_state, name);
+
+  driver_state_named.state.voltage = state.GetVoltage();
+  driver_state_named.state.current = state.GetCurrent();
+  driver_state_named.state.temperature = state.GetTemperature();
+  driver_state_named.state.heatsink_temperature = state.GetHeatsinkTemperature();
 }
 
-void PantherSystemRosInterface::UpdateMsgErrors(const CANErrors & can_errors)
+void SystemROSInterface::UpdateMsgErrors(const CANErrors & can_errors)
 {
   auto & driver_state = realtime_driver_state_publisher_->msg_;
-  auto & front_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_FRONT);
-  auto & rear_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_REAR);
 
   driver_state.error = can_errors.error;
   driver_state.write_pdo_cmds_error = can_errors.write_pdo_cmds_error;
   driver_state.read_pdo_motor_states_error = can_errors.read_pdo_motor_states_error;
   driver_state.read_pdo_driver_state_error = can_errors.read_pdo_driver_state_error;
 
-  front_driver_state.state.motor_states_data_timed_out =
-    can_errors.front_motor_states_data_timed_out;
-  rear_driver_state.state.motor_states_data_timed_out = can_errors.rear_motor_states_data_timed_out;
+  for (const auto & [name, driver_errors] : can_errors.driver_errors) {
+    auto & driver_state_named = GetDriverStateByName(driver_state, name);
 
-  front_driver_state.state.driver_state_data_timed_out =
-    can_errors.front_driver_state_data_timed_out;
-  rear_driver_state.state.driver_state_data_timed_out = can_errors.rear_driver_state_data_timed_out;
-
-  front_driver_state.state.can_error = can_errors.front_can_error;
-  rear_driver_state.state.can_error = can_errors.rear_can_error;
-
-  front_driver_state.state.heartbeat_timeout = can_errors.front_heartbeat_timeout;
-  rear_driver_state.state.heartbeat_timeout = can_errors.rear_heartbeat_timeout;
+    driver_state_named.state.motor_states_data_timed_out =
+      driver_errors.motor_states_data_timed_out;
+    driver_state_named.state.driver_state_data_timed_out =
+      driver_errors.driver_state_data_timed_out;
+    driver_state_named.state.can_error = driver_errors.can_error;
+    driver_state_named.state.heartbeat_timeout = driver_errors.heartbeat_timeout;
+  }
 }
 
-void PantherSystemRosInterface::PublishEStopStateMsg(const bool e_stop)
+void SystemROSInterface::PublishEStopStateMsg(const bool e_stop)
 {
   realtime_e_stop_state_publisher_->msg_.data = e_stop;
   if (realtime_e_stop_state_publisher_->trylock()) {
@@ -200,21 +182,21 @@ void PantherSystemRosInterface::PublishEStopStateMsg(const bool e_stop)
   }
 }
 
-void PantherSystemRosInterface::PublishEStopStateIfChanged(const bool e_stop)
+void SystemROSInterface::PublishEStopStateIfChanged(const bool e_stop)
 {
   if (realtime_e_stop_state_publisher_->msg_.data != e_stop) {
     PublishEStopStateMsg(e_stop);
   }
 }
 
-void PantherSystemRosInterface::PublishRobotDriverState()
+void SystemROSInterface::PublishRobotDriverState()
 {
   if (realtime_driver_state_publisher_->trylock()) {
     realtime_driver_state_publisher_->unlockAndPublish();
   }
 }
 
-void PantherSystemRosInterface::InitializeAndPublishIOStateMsg(
+void SystemROSInterface::InitializeAndPublishIOStateMsg(
   const std::unordered_map<GPIOPin, bool> & io_state)
 {
   for (const auto & [pin, pin_value] : io_state) {
@@ -226,7 +208,7 @@ void PantherSystemRosInterface::InitializeAndPublishIOStateMsg(
   }
 }
 
-void PantherSystemRosInterface::PublishIOState(const GPIOInfo & gpio_info)
+void SystemROSInterface::PublishIOState(const GPIOInfo & gpio_info)
 {
   const bool pin_value = (gpio_info.value == gpiod::line::value::ACTIVE);
 
@@ -239,7 +221,7 @@ void PantherSystemRosInterface::PublishIOState(const GPIOInfo & gpio_info)
   }
 }
 
-bool PantherSystemRosInterface::UpdateIOStateMsg(const GPIOPin pin, const bool pin_value)
+bool SystemROSInterface::UpdateIOStateMsg(const GPIOPin pin, const bool pin_value)
 {
   auto & io_state_msg = realtime_io_state_publisher_->msg_;
 
@@ -276,7 +258,7 @@ bool PantherSystemRosInterface::UpdateIOStateMsg(const GPIOPin pin, const bool p
   return true;
 }
 
-rclcpp::CallbackGroup::SharedPtr PantherSystemRosInterface::GetOrCreateNodeCallbackGroup(
+rclcpp::CallbackGroup::SharedPtr SystemROSInterface::GetOrCreateNodeCallbackGroup(
   const unsigned group_id, rclcpp::CallbackGroupType callback_group_type)
 {
   if (group_id == 0) {
@@ -301,19 +283,7 @@ rclcpp::CallbackGroup::SharedPtr PantherSystemRosInterface::GetOrCreateNodeCallb
   return callback_group;
 }
 
-void PantherSystemRosInterface::InitializeRobotDriverStateMsg()
-{
-  DriverStateNamedMsg front_driver_state;
-  DriverStateNamedMsg rear_driver_state;
-  front_driver_state.name = DriverStateNamedMsg::NAME_FRONT;
-  rear_driver_state.name = DriverStateNamedMsg::NAME_REAR;
-
-  auto & driver_state = realtime_driver_state_publisher_->msg_;
-  driver_state.driver_states.push_back(front_driver_state);
-  driver_state.driver_states.push_back(rear_driver_state);
-}
-
-DriverStateNamedMsg & PantherSystemRosInterface::GetDriverStateByName(
+DriverStateNamedMsg & SystemROSInterface::GetDriverStateByName(
   RobotDriverStateMsg & robot_driver_state, const std::string & name)
 {
   auto & driver_states = robot_driver_state.driver_states;
@@ -323,7 +293,10 @@ DriverStateNamedMsg & PantherSystemRosInterface::GetDriverStateByName(
     [&name](const DriverStateNamedMsg & msg) { return msg.name == name; });
 
   if (it == driver_states.end()) {
-    throw std::runtime_error("Driver with name '" + name + "' not found.");
+    DriverStateNamedMsg driver_state_named;
+    driver_state_named.name = name;
+    driver_states.push_back(driver_state_named);
+    it = driver_states.end() - 1;
   }
 
   return *it;
diff --git a/panther_hardware_interfaces/src/panther_system/ugv_system.cpp b/panther_hardware_interfaces/src/panther_system/ugv_system.cpp
new file mode 100644
index 00000000..fcdf497a
--- /dev/null
+++ b/panther_hardware_interfaces/src/panther_system/ugv_system.cpp
@@ -0,0 +1,550 @@
+// 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_hardware_interfaces/panther_system/ugv_system.hpp"
+
+#include <array>
+#include <chrono>
+#include <functional>
+#include <limits>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <diagnostic_updater/diagnostic_status_wrapper.hpp>
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+#include <rclcpp/logging.hpp>
+
+#include "panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/system_ros_interface.hpp"
+#include "panther_hardware_interfaces/utils.hpp"
+
+#include "panther_utils/common_utilities.hpp"
+#include "panther_utils/diagnostics.hpp"
+
+namespace panther_hardware_interfaces
+{
+
+CallbackReturn UGVSystem::on_init(const hardware_interface::HardwareInfo & hardware_info)
+{
+  if (hardware_interface::SystemInterface::on_init(hardware_info) != CallbackReturn::SUCCESS) {
+    return CallbackReturn::ERROR;
+  }
+
+  try {
+    CheckJointSize();
+    SortAndCheckJointNames();
+    SetInitialValues();
+    CheckInterfaces();
+  } catch (const std::runtime_error & e) {
+    RCLCPP_ERROR_STREAM(logger_, "An exception occurred while initializing: " << e.what());
+    return CallbackReturn::ERROR;
+  }
+
+  try {
+    ReadDrivetrainSettings();
+    ReadCANopenSettings();
+    ReadInitializationActivationAttempts();
+    ReadParametersAndCreateRoboteqErrorFilter();
+    ReadDriverStatesUpdateFrequency();
+  } catch (const std::invalid_argument & e) {
+    RCLCPP_ERROR_STREAM(
+      logger_, "An exception occurred while reading the parameters: " << e.what());
+    return CallbackReturn::ERROR;
+  }
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn UGVSystem::on_configure(const rclcpp_lifecycle::State &)
+{
+  try {
+    ConfigureGPIOController();
+    ConfigureRobotDriver();
+    ConfigureEStop();
+  } catch (const std::runtime_error & e) {
+    RCLCPP_ERROR_STREAM(
+      logger_, "Failed to initialize GPIO, Motors, or E-Stop controllers. Error: " << e.what());
+    return CallbackReturn::ERROR;
+  }
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn UGVSystem::on_cleanup(const rclcpp_lifecycle::State &)
+{
+  robot_driver_->Deinitialize();
+  robot_driver_.reset();
+
+  gpio_controller_.reset();
+  e_stop_.reset();
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn UGVSystem::on_activate(const rclcpp_lifecycle::State &)
+{
+  std::fill(hw_commands_velocities_.begin(), hw_commands_velocities_.end(), 0.0);
+  std::fill(hw_states_positions_.begin(), hw_states_positions_.end(), 0.0);
+  std::fill(hw_states_velocities_.begin(), hw_states_velocities_.end(), 0.0);
+  std::fill(hw_states_efforts_.begin(), hw_states_efforts_.end(), 0.0);
+
+  if (!OperationWithAttempts(
+        std::bind(&RobotDriverInterface::Activate, robot_driver_),
+        max_roboteq_activation_attempts_)) {
+    RCLCPP_ERROR_STREAM(
+      logger_, "Failed to activate UGV System: Couldn't activate RobotDriver in "
+                 << max_roboteq_activation_attempts_ << " attempts.");
+    return CallbackReturn::ERROR;
+  }
+
+  system_ros_interface_ = std::make_unique<SystemROSInterface>("hardware_controller");
+
+  system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
+    "~/fan_enable",
+    std::bind(&GPIOControllerInterface::FanEnable, gpio_controller_, std::placeholders::_1));
+  system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
+    "~/aux_power_enable",
+    std::bind(&GPIOControllerInterface::AUXPowerEnable, gpio_controller_, std::placeholders::_1));
+  system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
+    "~/digital_power_enable",
+    std::bind(
+      &GPIOControllerInterface::DigitalPowerEnable, gpio_controller_, std::placeholders::_1));
+  system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
+    "~/charger_enable",
+    std::bind(&GPIOControllerInterface::ChargerEnable, gpio_controller_, std::placeholders::_1));
+  system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
+    "~/led_control_enable",
+    std::bind(&GPIOControllerInterface::LEDControlEnable, gpio_controller_, std::placeholders::_1));
+  system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
+    "~/motor_power_enable", std::bind(&UGVSystem::MotorsPowerEnable, this, std::placeholders::_1));
+
+  system_ros_interface_->AddService<TriggerSrv, std::function<void()>>(
+    "~/e_stop_trigger", std::bind(&EStopInterface::TriggerEStop, e_stop_), 1,
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  auto e_stop_reset_qos = rmw_qos_profile_services_default;
+  e_stop_reset_qos.depth = 1;
+  system_ros_interface_->AddService<TriggerSrv, std::function<void()>>(
+    "~/e_stop_reset", std::bind(&EStopInterface::ResetEStop, e_stop_), 2,
+    rclcpp::CallbackGroupType::MutuallyExclusive, e_stop_reset_qos);
+
+  system_ros_interface_->AddDiagnosticTask(
+    std::string("system errors"), this, &UGVSystem::DiagnoseErrors);
+
+  system_ros_interface_->AddDiagnosticTask(
+    std::string("system status"), this, &UGVSystem::DiagnoseStatus);
+
+  gpio_controller_->RegisterGPIOEventCallback(
+    [this](const auto & state) { system_ros_interface_->PublishIOState(state); });
+
+  const auto io_state = gpio_controller_->QueryControlInterfaceIOStates();
+  system_ros_interface_->InitializeAndPublishIOStateMsg(io_state);
+
+  system_ros_interface_->PublishEStopStateMsg(e_stop_->ReadEStopState());
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn UGVSystem::on_deactivate(const rclcpp_lifecycle::State &)
+{
+  try {
+    e_stop_->TriggerEStop();
+  } catch (const std::runtime_error & e) {
+    RCLCPP_ERROR_STREAM(logger_, "Deactivation failed: " << e.what());
+    return CallbackReturn::ERROR;
+  }
+
+  system_ros_interface_.reset();
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn UGVSystem::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  try {
+    e_stop_->TriggerEStop();
+  } catch (const std::runtime_error & e) {
+    RCLCPP_ERROR_STREAM(logger_, "Shutdown failed: " << e.what());
+    return CallbackReturn::ERROR;
+  }
+
+  robot_driver_->Deinitialize();
+  robot_driver_.reset();
+
+  gpio_controller_.reset();
+
+  system_ros_interface_.reset();
+  e_stop_.reset();
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn UGVSystem::on_error(const rclcpp_lifecycle::State &)
+{
+  try {
+    e_stop_->TriggerEStop();
+  } catch (const std::runtime_error & e) {
+    RCLCPP_ERROR_STREAM(logger_, "Handling error failed: " << e.what());
+    return CallbackReturn::ERROR;
+  }
+
+  if (system_ros_interface_) {
+    system_ros_interface_->BroadcastOnDiagnosticTasks(
+      diagnostic_msgs::msg::DiagnosticStatus::ERROR,
+      "An error has occurred during a node state transition.");
+
+    system_ros_interface_.reset();
+  }
+
+  robot_driver_->Deinitialize();
+  robot_driver_.reset();
+
+  gpio_controller_.reset();
+  e_stop_.reset();
+
+  return CallbackReturn::SUCCESS;
+}
+
+std::vector<StateInterface> UGVSystem::export_state_interfaces()
+{
+  std::vector<StateInterface> state_interfaces;
+  for (std::size_t i = 0; i < joint_size_; i++) {
+    state_interfaces.emplace_back(StateInterface(
+      joints_names_sorted_[i], hardware_interface::HW_IF_POSITION, &hw_states_positions_[i]));
+    state_interfaces.emplace_back(StateInterface(
+      joints_names_sorted_[i], hardware_interface::HW_IF_VELOCITY, &hw_states_velocities_[i]));
+    state_interfaces.emplace_back(StateInterface(
+      joints_names_sorted_[i], hardware_interface::HW_IF_EFFORT, &hw_states_efforts_[i]));
+  }
+
+  return state_interfaces;
+}
+
+std::vector<CommandInterface> UGVSystem::export_command_interfaces()
+{
+  std::vector<CommandInterface> command_interfaces;
+  for (std::size_t i = 0; i < joint_size_; i++) {
+    command_interfaces.emplace_back(hardware_interface::CommandInterface(
+      joints_names_sorted_[i], hardware_interface::HW_IF_VELOCITY, &hw_commands_velocities_[i]));
+  }
+
+  return command_interfaces;
+}
+
+return_type UGVSystem::read(const rclcpp::Time & time, const rclcpp::Duration & /* period */)
+{
+  UpdateMotorsState();
+
+  if (time >= next_driver_state_update_time_) {
+    UpdateDriverState();
+    UpdateDriverStateMsg();
+    system_ros_interface_->PublishRobotDriverState();
+    next_driver_state_update_time_ = time + driver_states_update_period_;
+  }
+
+  UpdateEStopState();
+
+  return return_type::OK;
+}
+
+return_type UGVSystem::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
+{
+  const bool e_stop = e_stop_->ReadEStopState();
+
+  if (!e_stop) {
+    HandleRobotDriverWriteOperation([this] {
+      const auto speed_cmds = GetSpeedCommands();
+      robot_driver_->SendSpeedCommands(speed_cmds);
+    });
+  }
+
+  return return_type::OK;
+}
+
+void UGVSystem::CheckJointSize() const
+{
+  if (info_.joints.size() != joint_size_) {
+    throw std::runtime_error(
+      "Wrong number of joints defined: " + std::to_string(info_.joints.size()) + ", " +
+      std::to_string(joint_size_) + " expected.");
+  }
+}
+
+void UGVSystem::SortAndCheckJointNames()
+{
+  // Sort joints names - later hw_states and hw_commands are accessed by static indexes, so it
+  // is necessary to make sure that joints are in specific order and order of definitions in URDF
+  // doesn't matter
+  for (std::size_t i = 0; i < joint_size_; i++) {
+    std::size_t match_count = 0;
+
+    for (std::size_t j = 0; j < joint_size_; j++) {
+      if (CheckIfJointNameContainValidSequence(info_.joints[j].name, joint_order_[i])) {
+        joints_names_sorted_[i] = info_.joints[j].name;
+        ++match_count;
+      }
+    }
+
+    if (match_count != 1) {
+      throw std::runtime_error(
+        "There should be exactly one joint containing " + joint_order_[i] + ", " +
+        std::to_string(match_count) + " found.");
+    }
+  }
+}
+
+void UGVSystem::SetInitialValues()
+{
+  // It isn't safe to set command to NaN - sometimes it could be interpreted as Inf (although it
+  // shouldn't). In case of velocity, I think that setting the initial value to 0.0 is the best
+  // option.
+  hw_commands_velocities_.resize(joint_size_, 0.0);
+
+  hw_states_positions_.resize(joint_size_, std::numeric_limits<double>::quiet_NaN());
+  hw_states_velocities_.resize(joint_size_, std::numeric_limits<double>::quiet_NaN());
+  hw_states_efforts_.resize(joint_size_, std::numeric_limits<double>::quiet_NaN());
+}
+
+void UGVSystem::CheckInterfaces() const
+{
+  for (const hardware_interface::ComponentInfo & joint : info_.joints) {
+    // Commands
+    if (joint.command_interfaces.size() != 1) {
+      throw std::runtime_error(
+        "Joint " + joint.name + " has " + std::to_string(joint.command_interfaces.size()) +
+        " command interfaces. 1 expected.");
+    }
+
+    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) {
+      throw std::runtime_error(
+        "Joint " + joint.name + " has " + joint.command_interfaces[0].name +
+        " command interface. " + hardware_interface::HW_IF_VELOCITY + " expected.");
+    }
+
+    // States
+    if (joint.state_interfaces.size() != 3) {
+      throw std::runtime_error(
+        "Joint " + joint.name + " has " + std::to_string(joint.state_interfaces.size()) +
+        " state  " + (joint.state_interfaces.size() == 1 ? "interface." : "interfaces.") +
+        " 3 expected.");
+    }
+
+    if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
+      throw std::runtime_error(
+        "Joint " + joint.name + " has " + joint.state_interfaces[0].name +
+        " as first state interface. " + hardware_interface::HW_IF_POSITION + " expected.");
+    }
+
+    if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) {
+      throw std::runtime_error(
+        "Joint " + joint.name + " has " + joint.state_interfaces[1].name +
+        " as second state interface. " + hardware_interface::HW_IF_VELOCITY + " expected.");
+    }
+
+    if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) {
+      throw std::runtime_error(
+        "Joint " + joint.name + " has " + joint.state_interfaces[2].name +
+        " as third state interface. " + hardware_interface::HW_IF_EFFORT + " expected.");
+    }
+  }
+}
+
+void UGVSystem::ReadDrivetrainSettings()
+{
+  drivetrain_settings_.motor_torque_constant =
+    std::stof(info_.hardware_parameters["motor_torque_constant"]);
+  drivetrain_settings_.gear_ratio = std::stof(info_.hardware_parameters["gear_ratio"]);
+  drivetrain_settings_.gearbox_efficiency =
+    std::stof(info_.hardware_parameters["gearbox_efficiency"]);
+  drivetrain_settings_.encoder_resolution =
+    std::stof(info_.hardware_parameters["encoder_resolution"]);
+  drivetrain_settings_.max_rpm_motor_speed =
+    std::stof(info_.hardware_parameters["max_rpm_motor_speed"]);
+}
+
+void UGVSystem::ReadCANopenSettings()
+{
+  canopen_settings_.can_interface_name = info_.hardware_parameters["can_interface_name"];
+  canopen_settings_.master_can_id = std::stoi(info_.hardware_parameters["master_can_id"]);
+  canopen_settings_.pdo_motor_states_timeout_ms =
+    std::chrono::milliseconds(std::stoi(info_.hardware_parameters["pdo_motor_states_timeout_ms"]));
+  canopen_settings_.pdo_driver_state_timeout_ms =
+    std::chrono::milliseconds(std::stoi(info_.hardware_parameters["pdo_driver_state_timeout_ms"]));
+  canopen_settings_.sdo_operation_timeout_ms =
+    std::chrono::milliseconds(std::stoi(info_.hardware_parameters["sdo_operation_timeout_ms"]));
+
+  ReadCANopenSettingsDriverCANIDs();
+}
+
+void UGVSystem::ReadInitializationActivationAttempts()
+{
+  max_roboteq_initialization_attempts_ =
+    std::stoi(info_.hardware_parameters["max_roboteq_initialization_attempts"]);
+  max_roboteq_activation_attempts_ =
+    std::stoi(info_.hardware_parameters["max_roboteq_activation_attempts"]);
+}
+
+void UGVSystem::ReadParametersAndCreateRoboteqErrorFilter()
+{
+  const unsigned max_write_pdo_cmds_errors_count =
+    std::stoi(info_.hardware_parameters["max_write_pdo_cmds_errors_count"]);
+  const unsigned max_read_pdo_motor_states_errors_count =
+    std::stoi(info_.hardware_parameters["max_read_pdo_motor_states_errors_count"]);
+  const unsigned max_read_pdo_driver_state_errors_count =
+    std::stoi(info_.hardware_parameters["max_read_pdo_driver_state_errors_count"]);
+
+  roboteq_error_filter_ = std::make_shared<RoboteqErrorFilter>(
+    max_write_pdo_cmds_errors_count, max_read_pdo_motor_states_errors_count,
+    max_read_pdo_driver_state_errors_count, 1);
+}
+
+void UGVSystem::ReadDriverStatesUpdateFrequency()
+{
+  const float driver_states_update_frequency =
+    std::stof(info_.hardware_parameters["driver_states_update_frequency"]);
+  driver_states_update_period_ =
+    rclcpp::Duration::from_seconds(1.0f / driver_states_update_frequency);
+}
+
+void UGVSystem::ConfigureGPIOController()
+{
+  gpio_controller_ = GPIOControllerFactory::CreateGPIOController(1.2);
+  gpio_controller_->Start();
+
+  RCLCPP_INFO(logger_, "Successfully configured GPIO controller.");
+}
+
+void UGVSystem::ConfigureRobotDriver()
+{
+  robot_driver_write_mtx_ = std::make_shared<std::mutex>();
+  DefineRobotDriver();
+
+  if (!OperationWithAttempts(
+        std::bind(&RobotDriverInterface::Initialize, robot_driver_),
+        max_roboteq_initialization_attempts_,
+        std::bind(&RobotDriverInterface::Deinitialize, robot_driver_))) {
+    throw std::runtime_error("Roboteq drivers initialization failed.");
+  }
+
+  RCLCPP_INFO(logger_, "Successfully configured robot driver");
+}
+
+void UGVSystem::ConfigureEStop()
+{
+  if (!gpio_controller_ || !roboteq_error_filter_ || !robot_driver_ || !robot_driver_write_mtx_) {
+    throw std::runtime_error("Failed to configure E-Stop, make sure to setup entities first.");
+  }
+
+  e_stop_ = std::make_shared<EStop>(
+    gpio_controller_, roboteq_error_filter_, robot_driver_, robot_driver_write_mtx_,
+    std::bind(&UGVSystem::AreVelocityCommandsNearZero, this));
+
+  RCLCPP_INFO(logger_, "Successfully configured E-Stop");
+}
+
+void UGVSystem::UpdateMotorsState()
+{
+  try {
+    robot_driver_->UpdateMotorsState();
+    UpdateHwStates();
+    UpdateMotorsStateDataTimedOut();
+  } catch (const std::runtime_error & e) {
+    roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true);
+
+    RCLCPP_ERROR_STREAM_THROTTLE(
+      logger_, steady_clock_, 5000,
+      "An exception occurred while updating motors states: " << e.what());
+  }
+}
+
+void UGVSystem::UpdateDriverState()
+{
+  try {
+    robot_driver_->UpdateDriversState();
+    UpdateFlagErrors();
+    UpdateDriverStateDataTimedOut();
+  } catch (const std::runtime_error & e) {
+    roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true);
+
+    RCLCPP_ERROR_STREAM_THROTTLE(
+      logger_, steady_clock_, 5000,
+      "An exception occurred while updating drivers states: " << e.what());
+  }
+}
+
+void UGVSystem::UpdateEStopState()
+{
+  if (robot_driver_->CommunicationError()) {
+    e_stop_->TriggerEStop();
+  }
+
+  const bool e_stop = e_stop_->ReadEStopState();
+  system_ros_interface_->PublishEStopStateIfChanged(e_stop);
+}
+
+void UGVSystem::HandleRobotDriverWriteOperation(std::function<void()> write_operation)
+{
+  try {
+    {
+      std::unique_lock<std::mutex> robot_driver_write_lck(
+        *robot_driver_write_mtx_, std::defer_lock);
+      if (!robot_driver_write_lck.try_lock()) {
+        throw std::runtime_error(
+          "Can't acquire mutex for writing commands - E-stop is being triggered.");
+      }
+      write_operation();
+    }
+
+    roboteq_error_filter_->UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, false);
+  } catch (const std::runtime_error & e) {
+    RCLCPP_WARN_STREAM(logger_, "An exception occurred while writing commands: " << e.what());
+    roboteq_error_filter_->UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true);
+  }
+}
+
+bool UGVSystem::AreVelocityCommandsNearZero()
+{
+  for (const auto & cmd : hw_commands_velocities_) {
+    if (std::abs(cmd) > std::numeric_limits<double>::epsilon()) {
+      return false;
+    }
+  }
+  return true;
+}
+
+void UGVSystem::MotorsPowerEnable(const bool enable)
+{
+  try {
+    {
+      std::lock_guard<std::mutex> lck_g(*robot_driver_write_mtx_);
+
+      if (!enable) {
+        robot_driver_->TurnOnEStop();
+      } else {
+        robot_driver_->TurnOffEStop();
+      }
+    }
+
+    e_stop_->TriggerEStop();
+
+    roboteq_error_filter_->SetClearErrorsFlag();
+    roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false);
+  } catch (const std::runtime_error & e) {
+    RCLCPP_WARN_STREAM(logger_, "An exception occurred while enabling motors power: " << e.what());
+  }
+}
+
+}  // namespace panther_hardware_interfaces
diff --git a/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp b/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp
deleted file mode 100644
index 3b8c0829..00000000
--- a/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp
+++ /dev/null
@@ -1,777 +0,0 @@
-// Copyright 2023 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 <chrono>
-#include <cstdint>
-#include <stdexcept>
-#include <string>
-#include <thread>
-#include <vector>
-
-#include <gtest/gtest.h>
-
-#include <rclcpp/rclcpp.hpp>
-
-#include <hardware_interface/resource_manager.hpp>
-#include <hardware_interface/types/lifecycle_state_names.hpp>
-
-#include <lifecycle_msgs/msg/state.hpp>
-
-#include <panther_msgs/msg/robot_driver_state.hpp>
-
-#include <panther_utils/test/ros_test_utils.hpp>
-
-#include "utils/panther_system_test_utils.hpp"
-#include "utils/roboteqs_mock.hpp"
-
-using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
-
-class TestPantherSystem : public ::testing::Test
-{
-public:
-  TestPantherSystem() { pth_test_.Start(pth_test_.GetDefaultPantherSystemUrdf()); }
-  ~TestPantherSystem() { pth_test_.Stop(); }
-
-  void CheckInterfaces()
-  {
-    EXPECT_EQ(pth_test_.GetResourceManager()->system_components_size(), 1u);
-    ASSERT_EQ(pth_test_.GetResourceManager()->state_interface_keys().size(), 12u);
-
-    EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fl_wheel_joint/position"));
-    EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fr_wheel_joint/position"));
-    EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rl_wheel_joint/position"));
-    EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rr_wheel_joint/position"));
-
-    EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fl_wheel_joint/velocity"));
-    EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fr_wheel_joint/velocity"));
-    EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rl_wheel_joint/velocity"));
-    EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rr_wheel_joint/velocity"));
-
-    EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fl_wheel_joint/effort"));
-    EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fr_wheel_joint/effort"));
-    EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rl_wheel_joint/effort"));
-    EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rr_wheel_joint/effort"));
-
-    ASSERT_EQ(pth_test_.GetResourceManager()->command_interface_keys().size(), 4u);
-
-    EXPECT_TRUE(
-      pth_test_.GetResourceManager()->command_interface_exists("fl_wheel_joint/velocity"));
-    EXPECT_TRUE(
-      pth_test_.GetResourceManager()->command_interface_exists("fr_wheel_joint/velocity"));
-    EXPECT_TRUE(
-      pth_test_.GetResourceManager()->command_interface_exists("rl_wheel_joint/velocity"));
-    EXPECT_TRUE(
-      pth_test_.GetResourceManager()->command_interface_exists("rr_wheel_joint/velocity"));
-  }
-
-  void CheckInitialValues()
-  {
-    using hardware_interface::LoanedCommandInterface;
-    using hardware_interface::LoanedStateInterface;
-
-    LoanedStateInterface fl_s_p =
-      pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/position");
-    LoanedStateInterface fr_s_p =
-      pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/position");
-    LoanedStateInterface rl_s_p =
-      pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/position");
-    LoanedStateInterface rr_s_p =
-      pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/position");
-
-    LoanedStateInterface fl_s_v =
-      pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/velocity");
-    LoanedStateInterface fr_s_v =
-      pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/velocity");
-    LoanedStateInterface rl_s_v =
-      pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/velocity");
-    LoanedStateInterface rr_s_v =
-      pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/velocity");
-
-    LoanedStateInterface fl_s_e =
-      pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/effort");
-    LoanedStateInterface fr_s_e =
-      pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/effort");
-    LoanedStateInterface rl_s_e =
-      pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/effort");
-    LoanedStateInterface rr_s_e =
-      pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/effort");
-
-    LoanedCommandInterface fl_c_v =
-      pth_test_.GetResourceManager()->claim_command_interface("fl_wheel_joint/velocity");
-    LoanedCommandInterface fr_c_v =
-      pth_test_.GetResourceManager()->claim_command_interface("fr_wheel_joint/velocity");
-    LoanedCommandInterface rl_c_v =
-      pth_test_.GetResourceManager()->claim_command_interface("rl_wheel_joint/velocity");
-    LoanedCommandInterface rr_c_v =
-      pth_test_.GetResourceManager()->claim_command_interface("rr_wheel_joint/velocity");
-
-    ASSERT_FLOAT_EQ(fl_s_p.get_value(), 0.0);
-    ASSERT_FLOAT_EQ(fr_s_p.get_value(), 0.0);
-    ASSERT_FLOAT_EQ(rl_s_p.get_value(), 0.0);
-    ASSERT_FLOAT_EQ(rr_s_p.get_value(), 0.0);
-
-    ASSERT_FLOAT_EQ(fl_s_v.get_value(), 0.0);
-    ASSERT_FLOAT_EQ(fr_s_v.get_value(), 0.0);
-    ASSERT_FLOAT_EQ(rl_s_v.get_value(), 0.0);
-    ASSERT_FLOAT_EQ(rr_s_v.get_value(), 0.0);
-
-    ASSERT_FLOAT_EQ(fl_s_e.get_value(), 0.0);
-    ASSERT_FLOAT_EQ(fr_s_e.get_value(), 0.0);
-    ASSERT_FLOAT_EQ(rl_s_e.get_value(), 0.0);
-    ASSERT_FLOAT_EQ(rr_s_e.get_value(), 0.0);
-
-    ASSERT_FLOAT_EQ(fl_c_v.get_value(), 0.0);
-    ASSERT_FLOAT_EQ(fr_c_v.get_value(), 0.0);
-    ASSERT_FLOAT_EQ(rl_c_v.get_value(), 0.0);
-    ASSERT_FLOAT_EQ(rr_c_v.get_value(), 0.0);
-  }
-
-  // 100 Hz
-  const float period_ = 0.01;
-
-  panther_hardware_interfaces_test::PantherSystemTestUtils pth_test_;
-};
-
-// TRANSITIONS
-TEST_F(TestPantherSystem, ConfigureActivateFinalizePantherSystem)
-{
-  using panther_hardware_interfaces_test::kPantherSystemName;
-
-  // check if hardware is configured
-  auto status_map = pth_test_.GetResourceManager()->get_components_status();
-  ASSERT_EQ(
-    status_map[kPantherSystemName].state.label(),
-    hardware_interface::lifecycle_state_names::UNCONFIGURED);
-
-  try {
-    pth_test_.ConfigurePantherSystem();
-  } catch (const std::exception & e) {
-    FAIL() << "Exception caught when trying to ConfigurePantherSystem: " << e.what();
-    return;
-  }
-  status_map = pth_test_.GetResourceManager()->get_components_status();
-  ASSERT_EQ(
-    status_map[kPantherSystemName].state.label(),
-    hardware_interface::lifecycle_state_names::INACTIVE);
-
-  try {
-    pth_test_.ActivatePantherSystem();
-  } catch (const std::exception & e) {
-    FAIL() << "Exception caught when trying to ActivatePantherSystem: " << e.what();
-    return;
-  }
-  status_map = pth_test_.GetResourceManager()->get_components_status();
-  ASSERT_EQ(
-    status_map[kPantherSystemName].state.label(),
-    hardware_interface::lifecycle_state_names::ACTIVE);
-
-  // Check interfaces
-  CheckInterfaces();
-
-  // Check initial values
-  CheckInitialValues();
-
-  try {
-    pth_test_.ShutdownPantherSystem();
-  } catch (const std::exception & e) {
-    FAIL() << "Exception caught when trying to ShutdownPantherSystem: " << e.what();
-    return;
-  }
-  status_map = pth_test_.GetResourceManager()->get_components_status();
-  ASSERT_EQ(
-    status_map[kPantherSystemName].state.label(),
-    hardware_interface::lifecycle_state_names::FINALIZED);
-}
-
-TEST_F(TestPantherSystem, ConfigureActivateDeactivateDeconfigurePantherSystem)
-{
-  using panther_hardware_interfaces_test::kPantherSystemName;
-
-  auto status_map = pth_test_.GetResourceManager()->get_components_status();
-  ASSERT_EQ(
-    status_map[kPantherSystemName].state.label(),
-    hardware_interface::lifecycle_state_names::UNCONFIGURED);
-
-  try {
-    pth_test_.ConfigurePantherSystem();
-  } catch (const std::exception & e) {
-    FAIL() << "Exception caught when trying to ConfigurePantherSystem: " << e.what();
-    return;
-  }
-  status_map = pth_test_.GetResourceManager()->get_components_status();
-  ASSERT_EQ(
-    status_map[kPantherSystemName].state.label(),
-    hardware_interface::lifecycle_state_names::INACTIVE);
-
-  try {
-    pth_test_.ActivatePantherSystem();
-  } catch (const std::exception & e) {
-    FAIL() << "Exception caught when trying to ActivatePantherSystem: " << e.what();
-    return;
-  }
-  status_map = pth_test_.GetResourceManager()->get_components_status();
-  ASSERT_EQ(
-    status_map[kPantherSystemName].state.label(),
-    hardware_interface::lifecycle_state_names::ACTIVE);
-
-  // Check interfaces
-  CheckInterfaces();
-
-  // Check initial values
-  CheckInitialValues();
-
-  try {
-    pth_test_.DeactivatePantherSystem();
-  } catch (const std::exception & e) {
-    FAIL() << "Exception caught when trying to DeactivatePantherSystem: " << e.what();
-    return;
-  }
-  status_map = pth_test_.GetResourceManager()->get_components_status();
-  ASSERT_EQ(
-    status_map[kPantherSystemName].state.label(),
-    hardware_interface::lifecycle_state_names::INACTIVE);
-
-  try {
-    pth_test_.UnconfigurePantherSystem();
-  } catch (const std::exception & e) {
-    FAIL() << "Exception caught when trying to UnconfigurePantherSystem: " << e.what();
-    return;
-  }
-  status_map = pth_test_.GetResourceManager()->get_components_status();
-  ASSERT_EQ(
-    status_map[kPantherSystemName].state.label(),
-    hardware_interface::lifecycle_state_names::UNCONFIGURED);
-}
-
-// WRITING
-TEST_F(TestPantherSystem, WriteCommandsPantherSystem)
-{
-  using hardware_interface::LoanedCommandInterface;
-
-  using panther_hardware_interfaces_test::DriverChannel;
-
-  using panther_hardware_interfaces_test::kRadPerSecToRbtqCmd;
-
-  const float fl_v = 0.1;
-  const float fr_v = 0.2;
-  const float rl_v = 0.3;
-  const float rr_v = 0.4;
-
-  pth_test_.ConfigureActivatePantherSystem();
-
-  LoanedCommandInterface fl_c_v =
-    pth_test_.GetResourceManager()->claim_command_interface("fl_wheel_joint/velocity");
-  LoanedCommandInterface fr_c_v =
-    pth_test_.GetResourceManager()->claim_command_interface("fr_wheel_joint/velocity");
-  LoanedCommandInterface rl_c_v =
-    pth_test_.GetResourceManager()->claim_command_interface("rl_wheel_joint/velocity");
-  LoanedCommandInterface rr_c_v =
-    pth_test_.GetResourceManager()->claim_command_interface("rr_wheel_joint/velocity");
-
-  fl_c_v.set_value(fl_v);
-  fr_c_v.set_value(fr_v);
-  rl_c_v.set_value(rl_v);
-  rr_c_v.set_value(rr_v);
-
-  ASSERT_FLOAT_EQ(fl_c_v.get_value(), fl_v);
-  ASSERT_FLOAT_EQ(fr_c_v.get_value(), fr_v);
-  ASSERT_FLOAT_EQ(rl_c_v.get_value(), rl_v);
-  ASSERT_FLOAT_EQ(rr_c_v.get_value(), rr_v);
-
-  const auto TIME = rclcpp::Time(0);
-  const auto PERIOD = rclcpp::Duration::from_seconds(period_);
-
-  pth_test_.GetResourceManager()->write(TIME, PERIOD);
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-
-  ASSERT_EQ(
-    pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2),
-    static_cast<std::int32_t>(fl_v * kRadPerSecToRbtqCmd));
-  ASSERT_EQ(
-    pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1),
-    static_cast<std::int32_t>(fr_v * kRadPerSecToRbtqCmd));
-  ASSERT_EQ(
-    pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2),
-    static_cast<std::int32_t>(rl_v * kRadPerSecToRbtqCmd));
-  ASSERT_EQ(
-    pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1),
-    static_cast<std::int32_t>(rr_v * kRadPerSecToRbtqCmd));
-
-  pth_test_.ShutdownPantherSystem();
-}
-
-// READING
-TEST_F(TestPantherSystem, ReadFeedbackPantherSystem)
-{
-  using hardware_interface::LoanedStateInterface;
-
-  using panther_hardware_interfaces_test::DriverChannel;
-
-  using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
-  using panther_hardware_interfaces_test::kRbtqPosFbToRad;
-  using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
-
-  const std::int32_t fl_val = 100;
-  const std::int32_t fr_val = 200;
-  const std::int32_t rl_val = 300;
-  const std::int32_t rr_val = 400;
-
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL2, fl_val);
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL1, fr_val);
-  pth_test_.GetRoboteqsMock()->GetRearDriver()->SetPosition(DriverChannel::CHANNEL2, rl_val);
-  pth_test_.GetRoboteqsMock()->GetRearDriver()->SetPosition(DriverChannel::CHANNEL1, rr_val);
-
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL2, fl_val);
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL1, fr_val);
-  pth_test_.GetRoboteqsMock()->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL2, rl_val);
-  pth_test_.GetRoboteqsMock()->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL1, rr_val);
-
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL2, fl_val);
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL1, fr_val);
-  pth_test_.GetRoboteqsMock()->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL2, rl_val);
-  pth_test_.GetRoboteqsMock()->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL1, rr_val);
-
-  pth_test_.ConfigureActivatePantherSystem();
-
-  LoanedStateInterface fl_s_p =
-    pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/position");
-  LoanedStateInterface fr_s_p =
-    pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/position");
-  LoanedStateInterface rl_s_p =
-    pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/position");
-  LoanedStateInterface rr_s_p =
-    pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/position");
-
-  LoanedStateInterface fl_s_v =
-    pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/velocity");
-  LoanedStateInterface fr_s_v =
-    pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/velocity");
-  LoanedStateInterface rl_s_v =
-    pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/velocity");
-  LoanedStateInterface rr_s_v =
-    pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/velocity");
-
-  LoanedStateInterface fl_s_e =
-    pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/effort");
-  LoanedStateInterface fr_s_e =
-    pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/effort");
-  LoanedStateInterface rl_s_e =
-    pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/effort");
-  LoanedStateInterface rr_s_e =
-    pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/effort");
-
-  const auto TIME = rclcpp::Time(0, 0, RCL_ROS_TIME);
-  const auto PERIOD = rclcpp::Duration::from_seconds(period_);
-  try {
-    pth_test_.GetResourceManager()->read(TIME, PERIOD);
-  } catch (const std::exception & e) {
-    FAIL() << "Exception: " << e.what();
-    return;
-  }
-
-  ASSERT_FLOAT_EQ(fl_s_p.get_value(), fl_val * kRbtqPosFbToRad);
-  ASSERT_FLOAT_EQ(fr_s_p.get_value(), fr_val * kRbtqPosFbToRad);
-  ASSERT_FLOAT_EQ(rl_s_p.get_value(), rl_val * kRbtqPosFbToRad);
-  ASSERT_FLOAT_EQ(rr_s_p.get_value(), rr_val * kRbtqPosFbToRad);
-
-  ASSERT_FLOAT_EQ(fl_s_v.get_value(), fl_val * kRbtqVelFbToRadPerSec);
-  ASSERT_FLOAT_EQ(fr_s_v.get_value(), fr_val * kRbtqVelFbToRadPerSec);
-  ASSERT_FLOAT_EQ(rl_s_v.get_value(), rl_val * kRbtqVelFbToRadPerSec);
-  ASSERT_FLOAT_EQ(rr_s_v.get_value(), rr_val * kRbtqVelFbToRadPerSec);
-
-  ASSERT_FLOAT_EQ(fl_s_e.get_value(), fl_val * kRbtqCurrentFbToNewtonMeters);
-  ASSERT_FLOAT_EQ(fr_s_e.get_value(), fr_val * kRbtqCurrentFbToNewtonMeters);
-  ASSERT_FLOAT_EQ(rl_s_e.get_value(), rl_val * kRbtqCurrentFbToNewtonMeters);
-  ASSERT_FLOAT_EQ(rr_s_e.get_value(), rr_val * kRbtqCurrentFbToNewtonMeters);
-
-  pth_test_.ShutdownPantherSystem();
-}
-
-TEST_F(TestPantherSystem, ReadOtherRoboteqParamsPantherSystem)
-{
-  using hardware_interface::LoanedStateInterface;
-
-  const std::int16_t f_temp = 30;
-  const std::int16_t r_temp = 32;
-  const std::int16_t f_heatsink_temp = 31;
-  const std::int16_t r_heatsink_temp = 33;
-  const std::uint16_t f_volt = 400;
-  const std::uint16_t r_volt = 430;
-  const std::int16_t f_battery_current_1 = 10;
-  const std::int16_t r_battery_current_1 = 30;
-  const std::int16_t f_battery_current_2 = 30;
-  const std::int16_t r_battery_current_2 = 40;
-
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetTemperature(f_temp);
-  pth_test_.GetRoboteqsMock()->GetRearDriver()->SetTemperature(r_temp);
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetHeatsinkTemperature(f_heatsink_temp);
-  pth_test_.GetRoboteqsMock()->GetRearDriver()->SetHeatsinkTemperature(r_heatsink_temp);
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetVoltage(f_volt);
-  pth_test_.GetRoboteqsMock()->GetRearDriver()->SetVoltage(r_volt);
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetBatteryCurrent1(f_battery_current_1);
-  pth_test_.GetRoboteqsMock()->GetRearDriver()->SetBatteryCurrent1(r_battery_current_1);
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetBatteryCurrent2(f_battery_current_2);
-  pth_test_.GetRoboteqsMock()->GetRearDriver()->SetBatteryCurrent2(r_battery_current_2);
-
-  rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("hardware_interface_test_node");
-
-  pth_test_.ConfigureActivatePantherSystem();
-
-  RobotDriverStateMsg::SharedPtr state_msg;
-  unsigned state_msg_count = 0;
-  auto sub = node->create_subscription<RobotDriverStateMsg>(
-    panther_hardware_interfaces_test::kRobotDriverStateTopic, rclcpp::SensorDataQoS(),
-    [&](const RobotDriverStateMsg::SharedPtr msg) {
-      state_msg = msg;
-      ++state_msg_count;
-    });
-
-  std::this_thread::sleep_for(std::chrono::seconds(2));
-
-  auto simulated_time = node->get_clock()->now();
-  const auto PERIOD = rclcpp::Duration::from_seconds(period_);
-
-  try {
-    pth_test_.GetResourceManager()->read(simulated_time, PERIOD);
-    ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5)));
-  } catch (const std::exception & e) {
-    FAIL() << "Exception: " << e.what();
-    return;
-  }
-
-  ASSERT_TRUE(state_msg);
-
-  ASSERT_EQ(static_cast<std::int16_t>(state_msg->driver_states.at(0).state.temperature), f_temp);
-  ASSERT_EQ(static_cast<std::int16_t>(state_msg->driver_states.at(1).state.temperature), r_temp);
-
-  ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->driver_states.at(0).state.heatsink_temperature),
-    f_heatsink_temp);
-  ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->driver_states.at(1).state.heatsink_temperature),
-    r_heatsink_temp);
-
-  ASSERT_EQ(
-    static_cast<std::uint16_t>(state_msg->driver_states.at(0).state.voltage * 10.0), f_volt);
-  ASSERT_EQ(
-    static_cast<std::uint16_t>(state_msg->driver_states.at(1).state.voltage * 10.0), r_volt);
-
-  ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->driver_states.at(0).state.current * 10.0),
-    (f_battery_current_1 + f_battery_current_2));
-  ASSERT_EQ(
-    static_cast<std::int16_t>(state_msg->driver_states.at(1).state.current * 10.0),
-    (r_battery_current_1 + r_battery_current_2));
-
-  pth_test_.ShutdownPantherSystem();
-}
-
-// ENCODER DISCONNECTED
-TEST_F(TestPantherSystem, EncoderDisconnectedPantherSystem)
-{
-  using hardware_interface::LoanedCommandInterface;
-  using panther_hardware_interfaces_test::DriverChannel;
-  using panther_hardware_interfaces_test::DriverScriptFlags;
-
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetDriverScriptFlag(
-    DriverScriptFlags::ENCODER_DISCONNECTED);
-
-  rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("hardware_interface_test_node");
-
-  pth_test_.ConfigureActivatePantherSystem();
-
-  RobotDriverStateMsg::SharedPtr state_msg;
-  auto sub = node->create_subscription<RobotDriverStateMsg>(
-    panther_hardware_interfaces_test::kRobotDriverStateTopic, rclcpp::SensorDataQoS(),
-    [&](const RobotDriverStateMsg::SharedPtr msg) { state_msg = msg; });
-
-  std::this_thread::sleep_for(std::chrono::seconds(2));
-
-  const auto TIME = rclcpp::Time(0, 0, RCL_ROS_TIME);
-  const auto PERIOD = rclcpp::Duration::from_seconds(period_);
-
-  pth_test_.GetResourceManager()->read(TIME, PERIOD);
-
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5)));
-  ASSERT_TRUE(state_msg->driver_states.at(0).state.script_flag.encoder_disconnected);
-
-  // writing should be blocked - error
-
-  LoanedCommandInterface fl_c_v =
-    pth_test_.GetResourceManager()->claim_command_interface("fl_wheel_joint/velocity");
-  LoanedCommandInterface fr_c_v =
-    pth_test_.GetResourceManager()->claim_command_interface("fr_wheel_joint/velocity");
-  LoanedCommandInterface rl_c_v =
-    pth_test_.GetResourceManager()->claim_command_interface("rl_wheel_joint/velocity");
-  LoanedCommandInterface rr_c_v =
-    pth_test_.GetResourceManager()->claim_command_interface("rr_wheel_joint/velocity");
-
-  fl_c_v.set_value(0.1);
-  fr_c_v.set_value(0.1);
-  rl_c_v.set_value(0.1);
-  rr_c_v.set_value(0.1);
-
-  ASSERT_FLOAT_EQ(fl_c_v.get_value(), 0.1);
-  ASSERT_FLOAT_EQ(fr_c_v.get_value(), 0.1);
-  ASSERT_FLOAT_EQ(rl_c_v.get_value(), 0.1);
-  ASSERT_FLOAT_EQ(rr_c_v.get_value(), 0.1);
-
-  pth_test_.GetResourceManager()->write(TIME, PERIOD);
-
-  ASSERT_EQ(
-    pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0);
-  ASSERT_EQ(
-    pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0);
-  ASSERT_EQ(
-    pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0);
-  ASSERT_EQ(
-    pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0);
-
-  pth_test_.ShutdownPantherSystem();
-}
-
-// INITIAL PROCEDURE
-TEST_F(TestPantherSystem, InitialProcedureTestPantherSystem)
-{
-  using hardware_interface::LoanedStateInterface;
-  using panther_hardware_interfaces_test::DriverChannel;
-
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetRoboteqCmd(DriverChannel::CHANNEL1, 234);
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetRoboteqCmd(DriverChannel::CHANNEL2, 32);
-  pth_test_.GetRoboteqsMock()->GetRearDriver()->SetRoboteqCmd(DriverChannel::CHANNEL1, 54);
-  pth_test_.GetRoboteqsMock()->GetRearDriver()->SetRoboteqCmd(DriverChannel::CHANNEL2, 12);
-
-  pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetResetRoboteqScript(65);
-  pth_test_.GetRoboteqsMock()->GetRearDriver()->SetResetRoboteqScript(23);
-
-  pth_test_.ConfigureActivatePantherSystem();
-
-  ASSERT_EQ(pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetResetRoboteqScript(), 2);
-  ASSERT_EQ(pth_test_.GetRoboteqsMock()->GetRearDriver()->GetResetRoboteqScript(), 2);
-
-  ASSERT_EQ(
-    pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0);
-  ASSERT_EQ(
-    pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0);
-  ASSERT_EQ(
-    pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0);
-  ASSERT_EQ(
-    pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0);
-
-  pth_test_.ShutdownPantherSystem();
-}
-
-// ERROR HANDLING
-// TODO: FIX - return code -10, but otherwise seems to work
-// TEST(TestPantherSystemOthers, test_error_state)
-// {
-//   using panther_hardware_interfaces_test::kDefaultJoints;
-//   using panther_hardware_interfaces_test::kDefaultParamMap;
-//   using panther_hardware_interfaces_test::kPantherSystemName;
-
-//   panther_hardware_interfaces_test::PantherSystemTestUtils pth_test_;
-
-//   auto param_map = kDefaultParamMap;
-
-//   param_map["max_write_pdo_cmds_errors_count"] = "1";
-//   param_map["max_read_pdo_motor_states_errors_count"] = "1";
-//   param_map["max_read_pdo_driver_state_errors_count"] = "1";
-
-//   const std::string panther_system_urdf_ = pth_test_.BuildUrdf(param_map, kDefaultJoints);
-//   const float period_ = 0.01;
-
-//   pth_test_.Start(panther_system_urdf_);
-
-//   pth_test_.ConfigureActivatePantherSystem();
-
-//   pth_test_.GetRoboteqsMock()->GetFrontDriver()->StopPublishing();
-//   pth_test_.GetRoboteqsMock()->GetRearDriver()->StopPublishing();
-//   pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 200000);
-//   pth_test_.GetRoboteqsMock()->GetRearDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 200000);
-
-//   auto TIME = rclcpp::Time(0, 0, RCL_ROS_TIME);
-//   const auto PERIOD = rclcpp::Duration::from_seconds(period_);
-
-//   pth_test_.GetResourceManager()->read(TIME, PERIOD);
-//   pth_test_.GetResourceManager()->write(TIME, PERIOD);
-
-//   auto status_map = pth_test_.GetResourceManager()->get_components_status();
-//   ASSERT_EQ(
-//     status_map[kPantherSystemName].state.label(),
-//     hardware_interface::lifecycle_state_names::FINALIZED);
-
-//   pth_test_.Stop();
-// }
-
-// WRONG ORDER URDF
-TEST(TestPantherSystemOthers, WrongOrderURDF)
-{
-  using hardware_interface::LoanedCommandInterface;
-
-  using panther_hardware_interfaces_test::DriverChannel;
-
-  using panther_hardware_interfaces_test::kDefaultParamMap;
-  using panther_hardware_interfaces_test::kRadPerSecToRbtqCmd;
-
-  const float period_ = 0.01;
-
-  panther_hardware_interfaces_test::PantherSystemTestUtils pth_test_;
-
-  std::vector<std::string> joints = {
-    "rr_wheel_joint", "fl_wheel_joint", "fr_wheel_joint", "rl_wheel_joint"};
-
-  const std::string panther_system_urdf_ = pth_test_.BuildUrdf(kDefaultParamMap, joints);
-
-  pth_test_.Start(panther_system_urdf_);
-
-  const float fl_v = 0.1;
-  const float fr_v = 0.2;
-  const float rl_v = 0.3;
-  const float rr_v = 0.4;
-
-  pth_test_.ConfigureActivatePantherSystem();
-
-  // loaned command interfaces have to be destroyed before running Stop
-  {
-    LoanedCommandInterface fl_c_v =
-      pth_test_.GetResourceManager()->claim_command_interface("fl_wheel_joint/velocity");
-    LoanedCommandInterface fr_c_v =
-      pth_test_.GetResourceManager()->claim_command_interface("fr_wheel_joint/velocity");
-    LoanedCommandInterface rl_c_v =
-      pth_test_.GetResourceManager()->claim_command_interface("rl_wheel_joint/velocity");
-    LoanedCommandInterface rr_c_v =
-      pth_test_.GetResourceManager()->claim_command_interface("rr_wheel_joint/velocity");
-
-    fl_c_v.set_value(fl_v);
-    fr_c_v.set_value(fr_v);
-    rl_c_v.set_value(rl_v);
-    rr_c_v.set_value(rr_v);
-
-    ASSERT_FLOAT_EQ(fl_c_v.get_value(), fl_v);
-    ASSERT_FLOAT_EQ(fr_c_v.get_value(), fr_v);
-    ASSERT_FLOAT_EQ(rl_c_v.get_value(), rl_v);
-    ASSERT_FLOAT_EQ(rr_c_v.get_value(), rr_v);
-
-    const auto TIME = rclcpp::Time(0);
-    const auto PERIOD = rclcpp::Duration::from_seconds(period_);
-
-    pth_test_.GetResourceManager()->write(TIME, PERIOD);
-
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-
-    ASSERT_EQ(
-      pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2),
-      static_cast<std::int32_t>(fl_v * kRadPerSecToRbtqCmd));
-    ASSERT_EQ(
-      pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1),
-      static_cast<std::int32_t>(fr_v * kRadPerSecToRbtqCmd));
-    ASSERT_EQ(
-      pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2),
-      static_cast<std::int32_t>(rl_v * kRadPerSecToRbtqCmd));
-    ASSERT_EQ(
-      pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1),
-      static_cast<std::int32_t>(rr_v * kRadPerSecToRbtqCmd));
-  }
-
-  pth_test_.ShutdownPantherSystem();
-
-  pth_test_.Stop();
-}
-
-// TIMEOUT TESTS
-
-// TODO: fix
-// TEST(TestPantherSystemOthers, pdo_read_motors_states_timeout_test)
-// {
-//   using panther_hardware_interfaces_test::kDefaultJoints;
-//   using panther_hardware_interfaces_test::kDefaultParamMap;
-
-//   panther_hardware_interfaces_test::PantherSystemTestUtils pth_test_;
-
-//   auto param_map = kDefaultParamMap;
-
-//   // It is necessary to set max_read_pdo_errors_count to some higher value, because
-//   // adding wait time to Roboteq mock block all communication (also PDO), and PDO timeouts
-//   // happen
-//   param_map["pdo_motor_states_timeout_ms"] = "15";
-//   param_map["max_write_pdo_cmds_errors_count"] = "100";
-//   param_map["max_read_pdo_motor_states_errors_count"] = "2";
-//   param_map["max_read_pdo_driver_state_errors_count"] = "2";
-
-//   const std::string panther_system_urdf_ = pth_test_.BuildUrdf(param_map, kDefaultJoints);
-//   const float period_ = 0.01;
-
-//   pth_test_.Start(panther_system_urdf_);
-
-//   rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("hardware_interface_test_node");
-//   pth_test_.ConfigureActivatePantherSystem();
-
-//   RobotDriverStateMsg::SharedPtr state_msg;
-//   auto sub = node->create_subscription<RobotDriverStateMsg>(
-//     panther_hardware_interfaces_test::kRobotDriverStateTopic, rclcpp::SensorDataQoS(),
-//     [&](const RobotDriverStateMsg::SharedPtr msg) { state_msg = msg; });
-
-//   std::this_thread::sleep_for(std::chrono::seconds(2));
-
-//   auto TIME = rclcpp::Time(0, 0, RCL_ROS_TIME);
-//   const auto PERIOD = rclcpp::Duration::from_seconds(period_);
-
-//   pth_test_.GetResourceManager()->read(TIME, PERIOD);
-
-//   ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5)));
-//   ASSERT_FALSE(state_msg->read_pdo_motor_states_error);
-//   state_msg.reset();
-
-//   pth_test_.GetRoboteqsMock()->GetFrontDriver()->StopPublishing();
-//   pth_test_.GetRoboteqsMock()->GetRearDriver()->StopPublishing();
-
-//   pth_test_.GetResourceManager()->write(TIME, PERIOD);
-
-//   std::this_thread::sleep_for(PERIOD.to_chrono<std::chrono::milliseconds>());
-
-//   TIME += PERIOD;
-//   pth_test_.GetResourceManager()->read(TIME, PERIOD);
-
-//   ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5)));
-//   ASSERT_FALSE(state_msg->read_pdo_motor_states_error);
-//   state_msg.reset();
-
-//   pth_test_.GetResourceManager()->write(TIME, PERIOD);
-
-//   std::this_thread::sleep_for(PERIOD.to_chrono<std::chrono::milliseconds>());
-
-//   TIME += PERIOD;
-//   pth_test_.GetResourceManager()->read(TIME, PERIOD);
-
-//   ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5)));
-//   ASSERT_TRUE(state_msg->read_pdo_motor_states_error);
-
-//   pth_test_.ShutdownPantherSystem();
-
-//   pth_test_.Stop();
-// }
-
-// todo E-stop tests - it will the best to add them along with GPIO, as it will change the E-stop
-// procedure
-
-int main(int argc, char ** argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-
-  // For testing individual tests:
-  // testing::GTEST_FLAG(filter) = "TestPantherSystemOthers.pdo_read_motors_states_timeout_test";
-
-  return RUN_ALL_TESTS();
-}
diff --git a/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp b/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
deleted file mode 100644
index bf522dce..00000000
--- a/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp
+++ /dev/null
@@ -1,285 +0,0 @@
-// Copyright 2023 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 <algorithm>
-#include <chrono>
-#include <cstdint>
-#include <map>
-#include <memory>
-#include <string>
-
-#include <gtest/gtest.h>
-
-#include <rclcpp/rclcpp.hpp>
-
-#include <panther_utils/test/ros_test_utils.hpp>
-
-#include <panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp>
-
-#include "utils/test_constants.hpp"
-
-using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
-class TestPantherSystemRosInterface : public ::testing::Test
-{
-public:
-  TestPantherSystemRosInterface()
-  {
-    using panther_hardware_interfaces::PantherSystemRosInterface;
-
-    test_node_ = std::make_shared<rclcpp::Node>("test_panther_system_node");
-
-    driver_state_sub_ = test_node_->create_subscription<RobotDriverStateMsg>(
-      panther_hardware_interfaces_test::kRobotDriverStateTopic, rclcpp::SensorDataQoS(),
-      [&](const RobotDriverStateMsg::SharedPtr msg) { driver_state_msg_ = msg; });
-
-    panther_system_ros_interface_ =
-      std::make_unique<PantherSystemRosInterface>("hardware_controller");
-  }
-
-  ~TestPantherSystemRosInterface() { panther_system_ros_interface_.reset(); }
-
-protected:
-  rclcpp::Node::SharedPtr test_node_;
-  rclcpp::Subscription<RobotDriverStateMsg>::SharedPtr driver_state_sub_;
-  RobotDriverStateMsg::SharedPtr driver_state_msg_;
-  std::unique_ptr<panther_hardware_interfaces::PantherSystemRosInterface>
-    panther_system_ros_interface_;
-};
-
-TEST(TestPantherSystemRosInterfaceInitialization, NodeCreation)
-{
-  using panther_hardware_interfaces::PantherSystemRosInterface;
-
-  std::vector<std::string> node_names;
-  const std::string panther_system_node_name = "hardware_controller";
-  const std::string panther_system_node_name_with_ns = "/" + panther_system_node_name;
-
-  rclcpp::Node::SharedPtr test_node = std::make_shared<rclcpp::Node>("test_panther_system_node");
-
-  std::unique_ptr<PantherSystemRosInterface> panther_system_ros_interface;
-
-  panther_system_ros_interface =
-    std::make_unique<PantherSystemRosInterface>(panther_system_node_name);
-  node_names = test_node->get_node_names();
-  ASSERT_TRUE(
-    std::find(node_names.begin(), node_names.end(), panther_system_node_name_with_ns) !=
-    node_names.end());
-
-  panther_system_ros_interface.reset();
-  node_names = test_node->get_node_names();
-  ASSERT_FALSE(
-    std::find(node_names.begin(), node_names.end(), panther_system_node_name_with_ns) !=
-    node_names.end());
-
-  // Check if it is possible to create a node once again (if everything was cleaned up properly)
-  panther_system_ros_interface =
-    std::make_unique<PantherSystemRosInterface>(panther_system_node_name);
-  node_names = test_node->get_node_names();
-  ASSERT_TRUE(
-    std::find(node_names.begin(), node_names.end(), panther_system_node_name_with_ns) !=
-    node_names.end());
-
-  panther_system_ros_interface.reset();
-}
-
-TEST(TestPantherSystemRosInterfaceInitialization, Activation)
-{
-  using panther_hardware_interfaces::PantherSystemRosInterface;
-  using panther_hardware_interfaces_test::kRobotDriverStateTopic;
-
-  std::map<std::string, std::vector<std::string>> service_names_and_types;
-  std::map<std::string, std::vector<std::string>> topic_names_and_types;
-
-  rclcpp::Node::SharedPtr test_node = std::make_shared<rclcpp::Node>("test_panther_system_node");
-
-  std::unique_ptr<PantherSystemRosInterface> panther_system_ros_interface =
-    std::make_unique<PantherSystemRosInterface>("hardware_controller");
-
-  // Necessary to add some waiting, so that topic lists are updated
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-
-  topic_names_and_types = test_node->get_topic_names_and_types();
-  ASSERT_TRUE(topic_names_and_types.find(kRobotDriverStateTopic) != topic_names_and_types.end());
-
-  panther_system_ros_interface.reset();
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-
-  topic_names_and_types = test_node->get_topic_names_and_types();
-  ASSERT_FALSE(topic_names_and_types.find(kRobotDriverStateTopic) != topic_names_and_types.end());
-
-  panther_system_ros_interface = std::make_unique<PantherSystemRosInterface>("hardware_controller");
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-
-  topic_names_and_types = test_node->get_topic_names_and_types();
-  ASSERT_TRUE(topic_names_and_types.find(kRobotDriverStateTopic) != topic_names_and_types.end());
-
-  panther_system_ros_interface.reset();
-}
-
-TEST_F(TestPantherSystemRosInterface, ErrorFlags)
-{
-  panther_hardware_interfaces::RoboteqData front(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
-  panther_hardware_interfaces::RoboteqData rear(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
-
-  panther_hardware_interfaces::RoboteqDriverState front_driver_state;
-  front_driver_state.fault_flags = 0b00000001;
-  front_driver_state.script_flags = 0b00000010;
-  front_driver_state.runtime_stat_flag_motor_1 = 0b00001000;
-  front_driver_state.runtime_stat_flag_motor_2 = 0b00000100;
-
-  panther_hardware_interfaces::RoboteqDriverState rear_driver_state;
-  rear_driver_state.fault_flags = 0b00000010;
-  rear_driver_state.script_flags = 0b00000001;
-  rear_driver_state.runtime_stat_flag_motor_1 = 0b00100000;
-  rear_driver_state.runtime_stat_flag_motor_2 = 0b00010000;
-
-  front.SetDriverState(front_driver_state, false);
-  rear.SetDriverState(rear_driver_state, false);
-
-  panther_system_ros_interface_->UpdateMsgErrorFlags(front, rear);
-  panther_system_ros_interface_->PublishRobotDriverState();
-
-  ASSERT_TRUE(
-    panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5)));
-
-  EXPECT_TRUE(driver_state_msg_->driver_states.at(0).state.fault_flag.overheat);
-  EXPECT_TRUE(driver_state_msg_->driver_states.at(0).state.script_flag.encoder_disconnected);
-  EXPECT_TRUE(driver_state_msg_->driver_states.at(0).state.channel_2_motor_runtime_error.loop_error);
-  EXPECT_TRUE(
-    driver_state_msg_->driver_states.at(0).state.channel_1_motor_runtime_error.safety_stop_active);
-
-  EXPECT_TRUE(driver_state_msg_->driver_states.at(1).state.fault_flag.overvoltage);
-  EXPECT_TRUE(driver_state_msg_->driver_states.at(1).state.script_flag.loop_error);
-  EXPECT_TRUE(
-    driver_state_msg_->driver_states.at(1).state.channel_2_motor_runtime_error.forward_limit_triggered);
-  EXPECT_TRUE(driver_state_msg_->driver_states.at(1)
-                .state.channel_1_motor_runtime_error.reverse_limit_triggered);
-}
-
-TEST_F(TestPantherSystemRosInterface, DriversStates)
-{
-  panther_hardware_interfaces::DriverState front;
-  panther_hardware_interfaces::DriverState rear;
-
-  const std::int16_t f_temp = 36;
-  const std::int16_t f_heatsink_temp = 37;
-  const std::uint16_t f_volt = 405;
-  const std::int16_t f_battery_current_1 = 15;
-  const std::int16_t f_battery_current_2 = 12;
-  const std::int16_t r_temp = 35;
-  const std::int16_t r_heatsink_temp = 36;
-  const std::uint16_t r_volt = 404;
-  const std::int16_t r_battery_current_1 = 14;
-  const std::int16_t r_battery_current_2 = 11;
-
-  front.SetTemperature(f_temp);
-  front.SetHeatsinkTemperature(f_heatsink_temp);
-  front.SetVoltage(f_volt);
-  front.SetBatteryCurrent1(f_battery_current_1);
-  front.SetBatteryCurrent2(f_battery_current_2);
-
-  rear.SetTemperature(r_temp);
-  rear.SetHeatsinkTemperature(r_heatsink_temp);
-  rear.SetVoltage(r_volt);
-  rear.SetBatteryCurrent1(r_battery_current_1);
-  rear.SetBatteryCurrent2(r_battery_current_2);
-
-  panther_system_ros_interface_->UpdateMsgDriversStates(front, rear);
-  panther_system_ros_interface_->PublishRobotDriverState();
-
-  ASSERT_TRUE(
-    panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5)));
-
-  EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->driver_states.at(0).state.temperature), f_temp);
-  EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->driver_states.at(1).state.temperature), r_temp);
-
-  EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->driver_states.at(0).state.heatsink_temperature),
-    f_heatsink_temp);
-  EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->driver_states.at(1).state.heatsink_temperature),
-    r_heatsink_temp);
-
-  EXPECT_FLOAT_EQ(
-    static_cast<std::uint16_t>(driver_state_msg_->driver_states.at(0).state.voltage * 10.0),
-    f_volt);
-  EXPECT_FLOAT_EQ(
-    static_cast<std::uint16_t>(driver_state_msg_->driver_states.at(1).state.voltage * 10.0),
-    r_volt);
-
-  EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->driver_states.at(0).state.current * 10.0),
-    (f_battery_current_1 + f_battery_current_2));
-  EXPECT_FLOAT_EQ(
-    static_cast<std::int16_t>(driver_state_msg_->driver_states.at(1).state.current * 10.0),
-    (r_battery_current_1 + r_battery_current_2));
-}
-
-TEST_F(TestPantherSystemRosInterface, Errors)
-{
-  panther_hardware_interfaces::CANErrors can_errors;
-  can_errors.error = true;
-
-  can_errors.write_pdo_cmds_error = true;
-  can_errors.read_pdo_motor_states_error = false;
-  can_errors.read_pdo_driver_state_error = false;
-
-  can_errors.front_motor_states_data_timed_out = true;
-  can_errors.rear_motor_states_data_timed_out = false;
-
-  can_errors.front_driver_state_data_timed_out = false;
-  can_errors.rear_driver_state_data_timed_out = true;
-
-  can_errors.front_can_error = false;
-  can_errors.rear_can_error = true;
-
-  panther_system_ros_interface_->UpdateMsgErrors(can_errors);
-
-  panther_system_ros_interface_->PublishRobotDriverState();
-
-  ASSERT_TRUE(
-    panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5)));
-
-  EXPECT_TRUE(driver_state_msg_->error);
-
-  EXPECT_TRUE(driver_state_msg_->write_pdo_cmds_error);
-  EXPECT_FALSE(driver_state_msg_->read_pdo_motor_states_error);
-  EXPECT_FALSE(driver_state_msg_->read_pdo_driver_state_error);
-
-  EXPECT_TRUE(driver_state_msg_->driver_states.at(0).state.motor_states_data_timed_out);
-  EXPECT_FALSE(driver_state_msg_->driver_states.at(1).state.motor_states_data_timed_out);
-
-  EXPECT_FALSE(driver_state_msg_->driver_states.at(0).state.driver_state_data_timed_out);
-  EXPECT_TRUE(driver_state_msg_->driver_states.at(1).state.driver_state_data_timed_out);
-
-  EXPECT_FALSE(driver_state_msg_->driver_states.at(0).state.can_error);
-  EXPECT_TRUE(driver_state_msg_->driver_states.at(1).state.can_error);
-}
-
-int main(int argc, char ** argv)
-{
-  rclcpp::init(argc, argv);
-  testing::InitGoogleTest(&argc, argv);
-
-  auto run_tests = RUN_ALL_TESTS();
-
-  rclcpp::shutdown();
-  return run_tests;
-}
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
index 9c990a7f..714de2d9 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
+++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
@@ -44,21 +44,19 @@ class RoboteqRobotDriverWrapper : public panther_hardware_interfaces::RoboteqRob
   {
     // Assume 2 drivers and 4 motor drivers
     mock_fl_motor_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+      std::make_shared<panther_hardware_interfaces_test::MockMotorDriver::NiceMock>();
     mock_fr_motor_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+      std::make_shared<panther_hardware_interfaces_test::MockMotorDriver::NiceMock>();
     mock_rl_motor_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+      std::make_shared<panther_hardware_interfaces_test::MockMotorDriver::NiceMock>();
     mock_rr_motor_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+      std::make_shared<panther_hardware_interfaces_test::MockMotorDriver::NiceMock>();
 
-    mock_front_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
+    mock_front_driver = std::make_shared<panther_hardware_interfaces_test::MockDriver::NiceMock>();
     mock_front_driver->AddMotorDriver(kLeftMotorDriverName, mock_fl_motor_driver);
     mock_front_driver->AddMotorDriver(kRightMotorDriverName, mock_fr_motor_driver);
 
-    mock_rear_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
+    mock_rear_driver = std::make_shared<panther_hardware_interfaces_test::MockDriver::NiceMock>();
     mock_rear_driver->AddMotorDriver(kLeftMotorDriverName, mock_rl_motor_driver);
     mock_rear_driver->AddMotorDriver(kRightMotorDriverName, mock_rr_motor_driver);
   }
@@ -77,18 +75,12 @@ class RoboteqRobotDriverWrapper : public panther_hardware_interfaces::RoboteqRob
   static constexpr char kLeftMotorDriverName[] = "left";
   static constexpr char kRightMotorDriverName[] = "right";
 
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
-    mock_front_driver;
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
-    mock_rear_driver;
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-    mock_fl_motor_driver;
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-    mock_fr_motor_driver;
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-    mock_rl_motor_driver;
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
-    mock_rr_motor_driver;
+  std::shared_ptr<panther_hardware_interfaces_test::MockDriver::NiceMock> mock_front_driver;
+  std::shared_ptr<panther_hardware_interfaces_test::MockDriver::NiceMock> mock_rear_driver;
+  std::shared_ptr<panther_hardware_interfaces_test::MockMotorDriver::NiceMock> mock_fl_motor_driver;
+  std::shared_ptr<panther_hardware_interfaces_test::MockMotorDriver::NiceMock> mock_fr_motor_driver;
+  std::shared_ptr<panther_hardware_interfaces_test::MockMotorDriver::NiceMock> mock_rl_motor_driver;
+  std::shared_ptr<panther_hardware_interfaces_test::MockMotorDriver::NiceMock> mock_rr_motor_driver;
 };
 
 class TestRoboteqRobotDriverInitialization : public ::testing::Test
@@ -510,6 +502,29 @@ TEST_F(TestRoboteqRobotDriver, TurnOffEStopError)
   EXPECT_THROW(robot_driver_->TurnOffEStop(), std::runtime_error);
 }
 
+TEST_F(TestRoboteqRobotDriver, CommunicationError)
+{
+  EXPECT_CALL(*robot_driver_->mock_front_driver, IsHeartbeatTimeout())
+    .WillOnce(::testing::Return(false));
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, IsHeartbeatTimeout())
+    .WillOnce(::testing::Return(false));
+  EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).WillOnce(::testing::Return(false));
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, IsCANError()).WillOnce(::testing::Return(false));
+
+  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
+  EXPECT_FALSE(robot_driver_->CommunicationError());
+
+  EXPECT_CALL(*robot_driver_->mock_front_driver, IsHeartbeatTimeout())
+    .WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, IsHeartbeatTimeout())
+    .WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).WillOnce(::testing::Return(true));
+  EXPECT_CALL(*robot_driver_->mock_rear_driver, IsCANError()).WillOnce(::testing::Return(true));
+
+  ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState());
+  EXPECT_TRUE(robot_driver_->CommunicationError());
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/panther_hardware_interfaces/test/unit/panther_system/test_lynx_system.cpp b/panther_hardware_interfaces/test/unit/panther_system/test_lynx_system.cpp
new file mode 100644
index 00000000..ed1ecd65
--- /dev/null
+++ b/panther_hardware_interfaces/test/unit/panther_system/test_lynx_system.cpp
@@ -0,0 +1,311 @@
+// 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 <cstdint>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "panther_hardware_interfaces/panther_system/lynx_system.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
+
+#include "utils/system_test_utils.hpp"
+#include "utils/test_constants.hpp"
+
+class LynxSystemWrapper : public panther_hardware_interfaces::LynxSystem
+{
+public:
+  LynxSystemWrapper() : LynxSystem()
+  {
+    mock_robot_driver =
+      std::make_shared<panther_hardware_interfaces_test::MockRobotDriver::NiceMock>();
+    mock_gpio_controller =
+      std::make_shared<panther_hardware_interfaces_test::MockGPIOController::NiceMock>();
+    mock_e_stop = std::make_shared<panther_hardware_interfaces_test::MockEStop::NiceMock>();
+  }
+
+  void DefineRobotDriver() override { robot_driver_ = mock_robot_driver; }
+  void ConfigureGPIOController() override { gpio_controller_ = mock_gpio_controller; }
+  void ConfigureEStop() override { e_stop_ = mock_e_stop; }
+
+  void ReadCANopenSettingsDriverCANIDs() { LynxSystem::ReadCANopenSettingsDriverCANIDs(); }
+  void UpdateHwStates() { LynxSystem::UpdateHwStates(); }
+  void UpdateMotorsStateDataTimedOut() { LynxSystem::UpdateMotorsStateDataTimedOut(); }
+  void UpdateDriverStateDataTimedOut() { LynxSystem::UpdateDriverStateDataTimedOut(); }
+
+  void UpdateDriverStateMsg() { LynxSystem::UpdateDriverStateMsg(); }
+  void UpdateFlagErrors() { LynxSystem::UpdateFlagErrors(); }
+  std::vector<float> GetSpeedCommands() const { return LynxSystem::GetSpeedCommands(); }
+
+  void SetHwCommandsVelocities(std::vector<double> & velocities)
+  {
+    hw_commands_velocities_[0] = velocities[0];
+    hw_commands_velocities_[1] = velocities[1];
+    hw_commands_velocities_[2] = velocities[2];
+    hw_commands_velocities_[3] = velocities[3];
+  }
+
+  panther_hardware_interfaces::CANopenSettings GetCANopenSettings() { return canopen_settings_; }
+  std::vector<double> GetHwStatesPositions() { return hw_states_positions_; }
+  std::vector<double> GetHwStatesVelocities() { return hw_states_velocities_; }
+  std::vector<double> GetHwStatesEfforts() { return hw_states_efforts_; }
+
+  std::shared_ptr<panther_hardware_interfaces::RoboteqErrorFilter> GetRoboteqErrorFilter()
+  {
+    return roboteq_error_filter_;
+  }
+
+  std::shared_ptr<panther_hardware_interfaces_test::MockRobotDriver::NiceMock> mock_robot_driver;
+  std::shared_ptr<panther_hardware_interfaces_test::MockGPIOController::NiceMock>
+    mock_gpio_controller;
+  std::shared_ptr<panther_hardware_interfaces_test::MockEStop::NiceMock> mock_e_stop;
+};
+
+class TestLynxSystem : public ::testing::Test
+{
+public:
+  TestLynxSystem()
+  {
+    lynx_system_ = std::make_shared<LynxSystemWrapper>();
+
+    hardware_info_ = panther_hardware_interfaces_test::GenerateDefaultHardwareInfo();
+    hardware_info_.hardware_parameters.emplace("driver_can_id", "1");
+
+    lynx_system_->on_init(hardware_info_);
+    lynx_system_->on_configure(rclcpp_lifecycle::State());
+  }
+
+  ~TestLynxSystem() {}
+
+protected:
+  std::shared_ptr<LynxSystemWrapper> lynx_system_;
+  hardware_interface::HardwareInfo hardware_info_;
+};
+
+TEST_F(TestLynxSystem, ReadCANopenSettingsDriverCANIDs)
+{
+  ASSERT_NO_THROW(lynx_system_->ReadCANopenSettingsDriverCANIDs());
+
+  const auto canopen_settings = lynx_system_->GetCANopenSettings();
+
+  EXPECT_EQ(canopen_settings.driver_can_ids.size(), 1);
+  EXPECT_EQ(
+    canopen_settings.driver_can_ids.at(panther_hardware_interfaces::DriverNames::DEFAULT), 1);
+}
+
+TEST_F(TestLynxSystem, UpdateHwStates)
+{
+  const std::int32_t left_pos = 10;
+  const std::int16_t left_vel = 20;
+  const std::int16_t left_eff = 30;
+  const std::int32_t right_pos = 40;
+  const std::int16_t right_vel = 50;
+  const std::int16_t right_eff = 60;
+
+  const auto left_expected_pos = left_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad;
+  const auto left_expected_vel = left_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+  const auto left_expected_eff = left_eff *
+                                 panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  const auto right_expected_pos = right_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad;
+  const auto right_expected_vel = right_vel *
+                                  panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+  const auto right_expected_eff = right_eff *
+                                  panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+
+  panther_hardware_interfaces::MotorDriverState left_motor_driver_state = {
+    left_pos, left_vel, left_eff, {0, 0}, {0, 0}};
+  panther_hardware_interfaces::MotorDriverState right_motor_driver_state = {
+    right_pos, right_vel, right_eff, {0, 0}, {0, 0}};
+
+  panther_hardware_interfaces::DriverData roboteq_data(
+    panther_hardware_interfaces_test::kDrivetrainSettings);
+  // left - channel 2, right - channel 1
+  roboteq_data.SetMotorsStates(right_motor_driver_state, left_motor_driver_state, false);
+
+  EXPECT_CALL(
+    *lynx_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT)))
+    .WillOnce(::testing::ReturnRef(roboteq_data));
+
+  ASSERT_NO_THROW(lynx_system_->UpdateHwStates());
+
+  EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesPositions()[0], left_expected_pos);
+  EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesPositions()[1], right_expected_pos);
+  EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesPositions()[2], left_expected_pos);
+  EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesPositions()[3], right_expected_pos);
+
+  EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesVelocities()[0], left_expected_vel);
+  EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesVelocities()[1], right_expected_vel);
+  EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesVelocities()[2], left_expected_vel);
+  EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesVelocities()[3], right_expected_vel);
+
+  EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesEfforts()[0], left_expected_eff);
+  EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesEfforts()[1], right_expected_eff);
+  EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesEfforts()[2], left_expected_eff);
+  EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesEfforts()[3], right_expected_eff);
+}
+
+TEST_F(TestLynxSystem, UpdateMotorsStateDataTimedOut)
+{
+  panther_hardware_interfaces::MotorDriverState motor_driver_state;
+
+  panther_hardware_interfaces::DriverData roboteq_data(
+    panther_hardware_interfaces_test::kDrivetrainSettings);
+  roboteq_data.SetMotorsStates(motor_driver_state, motor_driver_state, true);
+
+  EXPECT_CALL(
+    *lynx_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT)))
+    .WillOnce(::testing::ReturnRef(roboteq_data));
+
+  lynx_system_->UpdateMotorsStateDataTimedOut();
+
+  auto error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap();
+  auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
+    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES));
+
+  EXPECT_TRUE(error);
+
+  // check if reset error works
+  roboteq_data.SetMotorsStates(motor_driver_state, motor_driver_state, false);
+
+  EXPECT_CALL(
+    *lynx_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT)))
+    .WillOnce(::testing::ReturnRef(roboteq_data));
+
+  lynx_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag();
+  lynx_system_->UpdateMotorsStateDataTimedOut();
+
+  error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap();
+  error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
+    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES));
+
+  EXPECT_FALSE(error);
+}
+
+TEST_F(TestLynxSystem, UpdateDriverStateMsg)
+{
+  // TODO requires subscribing to DriverState topic. Implement in the future or add abstraction for
+  // system_ros_interface_
+}
+
+TEST_F(TestLynxSystem, UpdateFlagErrors)
+{
+  auto driver_state = panther_hardware_interfaces::DriverState();
+  driver_state.fault_flags = 0b01;
+
+  panther_hardware_interfaces::DriverData roboteq_data(
+    panther_hardware_interfaces_test::kDrivetrainSettings);
+  roboteq_data.SetDriverState(driver_state, false);
+
+  EXPECT_CALL(
+    *lynx_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT)))
+    .WillOnce(::testing::ReturnRef(roboteq_data));
+
+  lynx_system_->UpdateFlagErrors();
+
+  auto error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap();
+  auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
+    panther_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER));
+
+  EXPECT_TRUE(error);
+
+  // check if reset error works
+  driver_state.fault_flags = 0;
+  roboteq_data.SetDriverState(driver_state, false);
+
+  EXPECT_CALL(
+    *lynx_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT)))
+    .WillOnce(::testing::ReturnRef(roboteq_data));
+
+  lynx_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag();
+  lynx_system_->UpdateFlagErrors();
+
+  error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap();
+  error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
+    panther_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER));
+
+  EXPECT_FALSE(error);
+}
+
+TEST_F(TestLynxSystem, UpdateDriverStateDataTimedOut)
+{
+  panther_hardware_interfaces::DriverState driver_state;
+
+  panther_hardware_interfaces::DriverData roboteq_data(
+    panther_hardware_interfaces_test::kDrivetrainSettings);
+  roboteq_data.SetDriverState(driver_state, true);
+
+  EXPECT_CALL(
+    *lynx_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT)))
+    .WillOnce(::testing::ReturnRef(roboteq_data));
+
+  lynx_system_->UpdateDriverStateDataTimedOut();
+
+  auto error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap();
+  auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
+    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE));
+
+  EXPECT_TRUE(error);
+
+  // check if reset error works
+  roboteq_data.SetDriverState(driver_state, false);
+
+  EXPECT_CALL(
+    *lynx_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT)))
+    .WillOnce(::testing::ReturnRef(roboteq_data));
+
+  lynx_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag();
+  lynx_system_->UpdateDriverStateDataTimedOut();
+
+  error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap();
+  error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
+    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE));
+
+  EXPECT_FALSE(error);
+}
+
+TEST_F(TestLynxSystem, GetSpeedCommands)
+{
+  const auto fl_v = 0.1;
+  const auto fr_v = 0.2;
+  const auto rl_v = 0.3;
+  const auto rr_v = 0.4;
+
+  std::vector<double> velocities = {fl_v, fr_v, rl_v, rr_v};
+  lynx_system_->SetHwCommandsVelocities(velocities);
+  const auto speed_cmd = lynx_system_->GetSpeedCommands();
+
+  // only front left and front right motors are used for speed commands
+  ASSERT_EQ(speed_cmd.size(), 2);
+  EXPECT_FLOAT_EQ(speed_cmd[0], fl_v);
+  EXPECT_FLOAT_EQ(speed_cmd[1], fr_v);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  const auto result = RUN_ALL_TESTS();
+  return result;
+}
diff --git a/panther_hardware_interfaces/test/unit/panther_system/test_panther_system.cpp b/panther_hardware_interfaces/test/unit/panther_system/test_panther_system.cpp
new file mode 100644
index 00000000..f75704ef
--- /dev/null
+++ b/panther_hardware_interfaces/test/unit/panther_system/test_panther_system.cpp
@@ -0,0 +1,375 @@
+// 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 <cstdint>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "panther_hardware_interfaces/panther_system/panther_system.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
+
+#include "utils/system_test_utils.hpp"
+#include "utils/test_constants.hpp"
+
+class PantherSystemWrapper : public panther_hardware_interfaces::PantherSystem
+{
+public:
+  PantherSystemWrapper() : PantherSystem()
+  {
+    mock_robot_driver =
+      std::make_shared<panther_hardware_interfaces_test::MockRobotDriver::NiceMock>();
+    mock_gpio_controller =
+      std::make_shared<panther_hardware_interfaces_test::MockGPIOController::NiceMock>();
+    mock_e_stop = std::make_shared<panther_hardware_interfaces_test::MockEStop::NiceMock>();
+
+    ON_CALL(
+      *mock_robot_driver, GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+      .WillByDefault(::testing::ReturnRef(default_driver_data));
+    ON_CALL(
+      *mock_robot_driver, GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+      .WillByDefault(::testing::ReturnRef(default_driver_data));
+  }
+
+  void DefineRobotDriver() override { robot_driver_ = mock_robot_driver; }
+  void ConfigureGPIOController() override { gpio_controller_ = mock_gpio_controller; }
+  void ConfigureEStop() override { e_stop_ = mock_e_stop; }
+
+  void ReadCANopenSettingsDriverCANIDs() { PantherSystem::ReadCANopenSettingsDriverCANIDs(); }
+  void UpdateHwStates() { PantherSystem::UpdateHwStates(); }
+  void UpdateMotorsStateDataTimedOut() { PantherSystem::UpdateMotorsStateDataTimedOut(); }
+  void UpdateDriverStateDataTimedOut() { PantherSystem::UpdateDriverStateDataTimedOut(); }
+
+  void UpdateDriverStateMsg() { PantherSystem::UpdateDriverStateMsg(); }
+  void UpdateFlagErrors() { PantherSystem::UpdateFlagErrors(); }
+  std::vector<float> GetSpeedCommands() const { return PantherSystem::GetSpeedCommands(); }
+
+  void SetHwCommandsVelocities(std::vector<double> & velocities)
+  {
+    hw_commands_velocities_[0] = velocities[0];
+    hw_commands_velocities_[1] = velocities[1];
+    hw_commands_velocities_[2] = velocities[2];
+    hw_commands_velocities_[3] = velocities[3];
+  }
+
+  panther_hardware_interfaces::CANopenSettings GetCANopenSettings() { return canopen_settings_; }
+  std::vector<double> GetHwStatesPositions() { return hw_states_positions_; }
+  std::vector<double> GetHwStatesVelocities() { return hw_states_velocities_; }
+  std::vector<double> GetHwStatesEfforts() { return hw_states_efforts_; }
+
+  std::shared_ptr<panther_hardware_interfaces::RoboteqErrorFilter> GetRoboteqErrorFilter()
+  {
+    return roboteq_error_filter_;
+  }
+
+  std::shared_ptr<panther_hardware_interfaces_test::MockRobotDriver::NiceMock> mock_robot_driver;
+  std::shared_ptr<panther_hardware_interfaces_test::MockGPIOController::NiceMock>
+    mock_gpio_controller;
+  std::shared_ptr<panther_hardware_interfaces_test::MockEStop::NiceMock> mock_e_stop;
+
+private:
+  panther_hardware_interfaces::DriverData default_driver_data =
+    panther_hardware_interfaces::DriverData(panther_hardware_interfaces_test::kDrivetrainSettings);
+};
+
+class TestPantherSystem : public ::testing::Test
+{
+public:
+  TestPantherSystem()
+  {
+    panther_system_ = std::make_shared<PantherSystemWrapper>();
+
+    hardware_info_ = panther_hardware_interfaces_test::GenerateDefaultHardwareInfo();
+    hardware_info_.hardware_parameters.emplace("front_driver_can_id", "1");
+    hardware_info_.hardware_parameters.emplace("rear_driver_can_id", "2");
+
+    panther_system_->on_init(hardware_info_);
+    panther_system_->on_configure(rclcpp_lifecycle::State());
+  }
+
+  ~TestPantherSystem() {}
+
+protected:
+  std::shared_ptr<PantherSystemWrapper> panther_system_;
+  hardware_interface::HardwareInfo hardware_info_;
+};
+
+TEST_F(TestPantherSystem, ReadCANopenSettingsDriverCANIDs)
+{
+  ASSERT_NO_THROW(panther_system_->ReadCANopenSettingsDriverCANIDs());
+
+  const auto canopen_settings = panther_system_->GetCANopenSettings();
+
+  EXPECT_EQ(canopen_settings.driver_can_ids.size(), 2);
+  EXPECT_EQ(canopen_settings.driver_can_ids.at(panther_hardware_interfaces::DriverNames::FRONT), 1);
+  EXPECT_EQ(canopen_settings.driver_can_ids.at(panther_hardware_interfaces::DriverNames::REAR), 2);
+}
+
+TEST_F(TestPantherSystem, UpdateHwStates)
+{
+  const std::int32_t fl_pos = 10;
+  const std::int16_t fl_vel = 20;
+  const std::int16_t fl_eff = 30;
+  const std::int32_t fr_pos = 40;
+  const std::int16_t fr_vel = 50;
+  const std::int16_t fr_eff = 60;
+  const std::int32_t rl_pos = 70;
+  const std::int16_t rl_vel = 80;
+  const std::int16_t rl_eff = 90;
+  const std::int32_t rr_pos = 100;
+  const std::int16_t rr_vel = 110;
+  const std::int16_t rr_eff = 120;
+
+  const auto fl_expected_pos = fl_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad;
+  const auto fl_expected_vel = fl_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+  const auto fl_expected_eff = fl_eff *
+                               panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  const auto fr_expected_pos = fr_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad;
+  const auto fr_expected_vel = fr_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+  const auto fr_expected_eff = fr_eff *
+                               panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  const auto rl_expected_pos = rl_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad;
+  const auto rl_expected_vel = rl_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+  const auto rl_expected_eff = rl_eff *
+                               panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  const auto rr_expected_pos = rr_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad;
+  const auto rr_expected_vel = rr_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+  const auto rr_expected_eff = rr_eff *
+                               panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+
+  panther_hardware_interfaces::MotorDriverState fl_motor_driver_state = {
+    fl_pos, fl_vel, fl_eff, {0, 0}, {0, 0}};
+  panther_hardware_interfaces::MotorDriverState fr_motor_driver_state = {
+    fr_pos, fr_vel, fr_eff, {0, 0}, {0, 0}};
+  panther_hardware_interfaces::MotorDriverState rl_motor_driver_state = {
+    rl_pos, rl_vel, rl_eff, {0, 0}, {0, 0}};
+  panther_hardware_interfaces::MotorDriverState rr_motor_driver_state = {
+    rr_pos, rr_vel, rr_eff, {0, 0}, {0, 0}};
+
+  panther_hardware_interfaces::DriverData front_roboteq_data(
+    panther_hardware_interfaces_test::kDrivetrainSettings);
+  panther_hardware_interfaces::DriverData rear_roboteq_data(
+    panther_hardware_interfaces_test::kDrivetrainSettings);
+  // left - channel 2, right - channel 1
+  front_roboteq_data.SetMotorsStates(fr_motor_driver_state, fl_motor_driver_state, false);
+  rear_roboteq_data.SetMotorsStates(rr_motor_driver_state, rl_motor_driver_state, false);
+
+  EXPECT_CALL(
+    *panther_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+    .WillOnce(::testing::ReturnRef(front_roboteq_data));
+  EXPECT_CALL(
+    *panther_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+    .WillOnce(::testing::ReturnRef(rear_roboteq_data));
+
+  ASSERT_NO_THROW(panther_system_->UpdateHwStates());
+
+  EXPECT_FLOAT_EQ(panther_system_->GetHwStatesPositions()[0], fl_expected_pos);
+  EXPECT_FLOAT_EQ(panther_system_->GetHwStatesPositions()[1], fr_expected_pos);
+  EXPECT_FLOAT_EQ(panther_system_->GetHwStatesPositions()[2], rl_expected_pos);
+  EXPECT_FLOAT_EQ(panther_system_->GetHwStatesPositions()[3], rr_expected_pos);
+
+  EXPECT_FLOAT_EQ(panther_system_->GetHwStatesVelocities()[0], fl_expected_vel);
+  EXPECT_FLOAT_EQ(panther_system_->GetHwStatesVelocities()[1], fr_expected_vel);
+  EXPECT_FLOAT_EQ(panther_system_->GetHwStatesVelocities()[2], rl_expected_vel);
+  EXPECT_FLOAT_EQ(panther_system_->GetHwStatesVelocities()[3], rr_expected_vel);
+
+  EXPECT_FLOAT_EQ(panther_system_->GetHwStatesEfforts()[0], fl_expected_eff);
+  EXPECT_FLOAT_EQ(panther_system_->GetHwStatesEfforts()[1], fr_expected_eff);
+  EXPECT_FLOAT_EQ(panther_system_->GetHwStatesEfforts()[2], rl_expected_eff);
+  EXPECT_FLOAT_EQ(panther_system_->GetHwStatesEfforts()[3], rr_expected_eff);
+}
+
+TEST_F(TestPantherSystem, UpdateMotorsStateDataTimedOut)
+{
+  panther_hardware_interfaces::MotorDriverState motor_driver_state;
+
+  panther_hardware_interfaces::DriverData roboteq_data(
+    panther_hardware_interfaces_test::kDrivetrainSettings);
+  roboteq_data.SetMotorsStates(motor_driver_state, motor_driver_state, true);
+
+  EXPECT_CALL(
+    *panther_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+    .WillOnce(::testing::ReturnRef(roboteq_data));
+  EXPECT_CALL(
+    *panther_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+    .Times(0);
+
+  panther_system_->UpdateMotorsStateDataTimedOut();
+
+  auto error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap();
+  auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
+    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES));
+
+  EXPECT_TRUE(error);
+
+  // check if reset error works
+  roboteq_data.SetMotorsStates(motor_driver_state, motor_driver_state, false);
+
+  EXPECT_CALL(
+    *panther_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+    .Times(1);
+  EXPECT_CALL(
+    *panther_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+    .Times(1);
+
+  panther_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag();
+  panther_system_->UpdateMotorsStateDataTimedOut();
+
+  error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap();
+  error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
+    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES));
+
+  EXPECT_FALSE(error);
+}
+
+TEST_F(TestPantherSystem, UpdateDriverStateMsg)
+{
+  // TODO requires subscribing to DriverState topic. Implement in the future or add abstraction for
+  // system_ros_interface_
+}
+
+TEST_F(TestPantherSystem, UpdateFlagErrors)
+{
+  panther_hardware_interfaces::DriverState driver_state;
+  driver_state.fault_flags = 0b01;
+  driver_state.script_flags = 0;
+  driver_state.runtime_stat_flag_channel_1 = 0;
+  driver_state.runtime_stat_flag_channel_2 = 0;
+
+  panther_hardware_interfaces::DriverData roboteq_data(
+    panther_hardware_interfaces_test::kDrivetrainSettings);
+  roboteq_data.SetDriverState(driver_state, false);
+
+  EXPECT_CALL(
+    *panther_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+    .Times(1);
+  EXPECT_CALL(
+    *panther_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+    .WillOnce(::testing::ReturnRef(roboteq_data));
+
+  panther_system_->UpdateFlagErrors();
+
+  auto error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap();
+  auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
+    panther_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER));
+
+  EXPECT_TRUE(error);
+
+  // check if reset error works
+  driver_state.fault_flags = 0;
+  roboteq_data.SetDriverState(driver_state, false);
+
+  EXPECT_CALL(
+    *panther_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+    .Times(1);
+  EXPECT_CALL(
+    *panther_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+    .WillOnce(::testing::ReturnRef(roboteq_data));
+
+  panther_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag();
+  panther_system_->UpdateFlagErrors();
+
+  error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap();
+  error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
+    panther_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER));
+
+  EXPECT_FALSE(error);
+}
+
+TEST_F(TestPantherSystem, UpdateDriverStateDataTimedOut)
+{
+  panther_hardware_interfaces::DriverState driver_state;
+
+  panther_hardware_interfaces::DriverData roboteq_data(
+    panther_hardware_interfaces_test::kDrivetrainSettings);
+  roboteq_data.SetDriverState(driver_state, true);
+
+  EXPECT_CALL(
+    *panther_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+    .WillOnce(::testing::ReturnRef(roboteq_data));
+  EXPECT_CALL(
+    *panther_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+    .Times(0);
+
+  panther_system_->UpdateDriverStateDataTimedOut();
+
+  auto error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap();
+  auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
+    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE));
+
+  EXPECT_TRUE(error);
+
+  // check if reset error works
+  roboteq_data.SetDriverState(driver_state, false);
+
+  EXPECT_CALL(
+    *panther_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+    .WillOnce(::testing::ReturnRef(roboteq_data));
+  EXPECT_CALL(
+    *panther_system_->mock_robot_driver,
+    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+    .Times(1);
+
+  panther_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag();
+  panther_system_->UpdateDriverStateDataTimedOut();
+
+  error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap();
+  error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
+    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE));
+
+  EXPECT_FALSE(error);
+}
+
+TEST_F(TestPantherSystem, GetSpeedCommands)
+{
+  const auto fl_v = 0.1;
+  const auto fr_v = 0.2;
+  const auto rl_v = 0.3;
+  const auto rr_v = 0.4;
+
+  std::vector<double> velocities = {fl_v, fr_v, rl_v, rr_v};
+  panther_system_->SetHwCommandsVelocities(velocities);
+  const auto speed_cmd = panther_system_->GetSpeedCommands();
+
+  ASSERT_EQ(speed_cmd.size(), 4);
+  EXPECT_FLOAT_EQ(speed_cmd[0], fl_v);
+  EXPECT_FLOAT_EQ(speed_cmd[1], fr_v);
+  EXPECT_FLOAT_EQ(speed_cmd[2], rl_v);
+  EXPECT_FLOAT_EQ(speed_cmd[3], rr_v);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  const auto result = RUN_ALL_TESTS();
+  return result;
+}
diff --git a/panther_hardware_interfaces/test/unit/panther_system/test_system_ros_interface.cpp b/panther_hardware_interfaces/test/unit/panther_system/test_system_ros_interface.cpp
new file mode 100644
index 00000000..68e2fbff
--- /dev/null
+++ b/panther_hardware_interfaces/test/unit/panther_system/test_system_ros_interface.cpp
@@ -0,0 +1,207 @@
+// 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 <algorithm>
+#include <cstdint>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <panther_hardware_interfaces/panther_system/system_ros_interface.hpp>
+
+#include "utils/test_constants.hpp"
+
+using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
+
+class SystemROSInterfaceWrapper : public panther_hardware_interfaces::SystemROSInterface
+{
+public:
+  SystemROSInterfaceWrapper(const std::string & node_name) : SystemROSInterface(node_name) {}
+
+  RobotDriverStateMsg & GetRobotDriverStateMsg() { return realtime_driver_state_publisher_->msg_; }
+};
+
+class TestSystemROSInterface : public ::testing::Test
+{
+public:
+  TestSystemROSInterface()
+  {
+    system_ros_interface_ = std::make_unique<SystemROSInterfaceWrapper>("hardware_controller");
+  }
+
+  ~TestSystemROSInterface() { system_ros_interface_.reset(); }
+
+protected:
+  std::unique_ptr<SystemROSInterfaceWrapper> system_ros_interface_;
+};
+
+TEST(TestSystemROSInterfaceInitialization, NodeCreation)
+{
+  using panther_hardware_interfaces::SystemROSInterface;
+
+  std::vector<std::string> node_names;
+  const std::string system_node_name = "hardware_controller";
+  const std::string system_node_name_with_ns = "/" + system_node_name;
+
+  rclcpp::Node::SharedPtr test_node = std::make_shared<rclcpp::Node>("test_system_node");
+
+  std::unique_ptr<SystemROSInterface> system_ros_interface;
+
+  system_ros_interface = std::make_unique<SystemROSInterface>(system_node_name);
+
+  node_names = test_node->get_node_names();
+  ASSERT_TRUE(
+    std::find(node_names.begin(), node_names.end(), system_node_name_with_ns) != node_names.end());
+
+  system_ros_interface.reset();
+  node_names = test_node->get_node_names();
+  ASSERT_FALSE(
+    std::find(node_names.begin(), node_names.end(), system_node_name_with_ns) != node_names.end());
+
+  // Check if it is possible to create a node once again (if everything was cleaned up properly)
+  system_ros_interface = std::make_unique<SystemROSInterface>(system_node_name);
+  node_names = test_node->get_node_names();
+  ASSERT_TRUE(
+    std::find(node_names.begin(), node_names.end(), system_node_name_with_ns) != node_names.end());
+}
+
+TEST_F(TestSystemROSInterface, UpdateMsgErrorFlags)
+{
+  panther_hardware_interfaces::DriverData data(
+    panther_hardware_interfaces_test::kDrivetrainSettings);
+
+  panther_hardware_interfaces::DriverState driver_state;
+  driver_state.fault_flags = 0b00000001;
+  driver_state.script_flags = 0b00000010;
+  driver_state.runtime_stat_flag_channel_1 = 0b00001000;
+  driver_state.runtime_stat_flag_channel_2 = 0b00000100;
+
+  data.SetDriverState(driver_state, false);
+
+  system_ros_interface_->UpdateMsgErrorFlags("driver", data);
+
+  const auto driver_state_msg = system_ros_interface_->GetRobotDriverStateMsg();
+
+  EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.fault_flag.overheat);
+  EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.script_flag.encoder_disconnected);
+  EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.channel_2_motor_runtime_error.loop_error);
+  EXPECT_TRUE(
+    driver_state_msg.driver_states.at(0).state.channel_1_motor_runtime_error.safety_stop_active);
+}
+
+TEST_F(TestSystemROSInterface, UpdateMsgDriversStates)
+{
+  panther_hardware_interfaces::RoboteqDriverState state;
+
+  const std::int16_t temp = 36;
+  const std::int16_t heatsink_temp = 37;
+  const std::uint16_t volt = 405;
+  const std::int16_t battery_current_1 = 15;
+  const std::int16_t battery_current_2 = 12;
+
+  state.SetTemperature(temp);
+  state.SetHeatsinkTemperature(heatsink_temp);
+  state.SetVoltage(volt);
+  state.SetBatteryCurrent1(battery_current_1);
+  state.SetBatteryCurrent2(battery_current_2);
+
+  system_ros_interface_->UpdateMsgDriversStates("driver", state);
+
+  const auto driver_state_msg = system_ros_interface_->GetRobotDriverStateMsg();
+
+  EXPECT_FLOAT_EQ(
+    static_cast<std::int16_t>(driver_state_msg.driver_states.at(0).state.temperature), temp);
+  EXPECT_FLOAT_EQ(
+    static_cast<std::int16_t>(driver_state_msg.driver_states.at(0).state.heatsink_temperature),
+    heatsink_temp);
+  EXPECT_FLOAT_EQ(
+    static_cast<std::uint16_t>(driver_state_msg.driver_states.at(0).state.voltage * 10.0), volt);
+  EXPECT_FLOAT_EQ(
+    static_cast<std::int16_t>(driver_state_msg.driver_states.at(0).state.current * 10.0),
+    (battery_current_1 + battery_current_2));
+}
+
+TEST_F(TestSystemROSInterface, UpdateMsgErrors)
+{
+  panther_hardware_interfaces::CANErrors can_errors;
+  can_errors.error = true;
+
+  can_errors.write_pdo_cmds_error = true;
+  can_errors.read_pdo_motor_states_error = false;
+  can_errors.read_pdo_driver_state_error = false;
+
+  panther_hardware_interfaces::DriverCANErrors driver_can_errors;
+  driver_can_errors.motor_states_data_timed_out = true;
+  driver_can_errors.driver_state_data_timed_out = false;
+  driver_can_errors.can_error = false;
+  driver_can_errors.heartbeat_timeout = true;
+
+  can_errors.driver_errors.emplace("driver", driver_can_errors);
+
+  system_ros_interface_->UpdateMsgErrors(can_errors);
+
+  const auto driver_state_msg = system_ros_interface_->GetRobotDriverStateMsg();
+
+  EXPECT_TRUE(driver_state_msg.error);
+
+  EXPECT_TRUE(driver_state_msg.write_pdo_cmds_error);
+  EXPECT_FALSE(driver_state_msg.read_pdo_motor_states_error);
+  EXPECT_FALSE(driver_state_msg.read_pdo_driver_state_error);
+
+  EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.motor_states_data_timed_out);
+  EXPECT_FALSE(driver_state_msg.driver_states.at(0).state.driver_state_data_timed_out);
+  EXPECT_FALSE(driver_state_msg.driver_states.at(0).state.can_error);
+  EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.heartbeat_timeout);
+}
+
+TEST_F(TestSystemROSInterface, CreateDriverStateEntryInMsg)
+{
+  const auto driver_1_name = "driver_1";
+  const auto driver_2_name = "driver_2";
+  const auto driver_3_name = "driver_3";
+
+  auto & driver_state_msg = system_ros_interface_->GetRobotDriverStateMsg();
+
+  ASSERT_EQ(driver_state_msg.driver_states.size(), 0);
+
+  // check 3 different methods that can create a new entry in the message
+  system_ros_interface_->UpdateMsgErrorFlags(
+    driver_1_name,
+    panther_hardware_interfaces::DriverData(panther_hardware_interfaces_test::kDrivetrainSettings));
+  system_ros_interface_->UpdateMsgDriversStates(driver_2_name, {});
+
+  panther_hardware_interfaces::CANErrors can_errors;
+  can_errors.driver_errors.emplace(driver_3_name, panther_hardware_interfaces::DriverCANErrors());
+  system_ros_interface_->UpdateMsgErrors(can_errors);
+
+  EXPECT_EQ(driver_state_msg.driver_states.size(), 3);
+  EXPECT_EQ(driver_state_msg.driver_states.at(0).name, driver_1_name);
+  EXPECT_EQ(driver_state_msg.driver_states.at(1).name, driver_2_name);
+  EXPECT_EQ(driver_state_msg.driver_states.at(2).name, driver_3_name);
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  testing::InitGoogleTest(&argc, argv);
+
+  auto run_tests = RUN_ALL_TESTS();
+
+  rclcpp::shutdown();
+  return run_tests;
+}
diff --git a/panther_hardware_interfaces/test/unit/panther_system/test_ugv_system.cpp b/panther_hardware_interfaces/test/unit/panther_system/test_ugv_system.cpp
new file mode 100644
index 00000000..0474f8cd
--- /dev/null
+++ b/panther_hardware_interfaces/test/unit/panther_system/test_ugv_system.cpp
@@ -0,0 +1,378 @@
+// 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 <cstdint>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include "panther_hardware_interfaces/panther_system/ugv_system.hpp"
+
+#include "utils/system_test_utils.hpp"
+
+class MockUGVSystem : public panther_hardware_interfaces::UGVSystem
+{
+public:
+  MockUGVSystem(const std::vector<std::string> & joint_order)
+  : panther_hardware_interfaces::UGVSystem(joint_order)
+  {
+    mock_robot_driver_ =
+      std::make_shared<panther_hardware_interfaces_test::MockRobotDriver::NiceMock>();
+    mock_gpio_controller_ =
+      std::make_shared<panther_hardware_interfaces_test::MockGPIOController::NiceMock>();
+    mock_e_stop_ = std::make_shared<panther_hardware_interfaces_test::MockEStop::NiceMock>();
+
+    ON_CALL(*this, DefineRobotDriver()).WillByDefault(::testing::Invoke([&]() {
+      robot_driver_ = mock_robot_driver_;
+    }));
+    ON_CALL(*this, ConfigureGPIOController()).WillByDefault(::testing::Invoke([&]() {
+      gpio_controller_ = mock_gpio_controller_;
+    }));
+    ON_CALL(*this, ConfigureEStop()).WillByDefault(::testing::Invoke([&]() {
+      e_stop_ = mock_e_stop_;
+    }));
+  }
+
+  MOCK_METHOD(void, ReadCANopenSettingsDriverCANIDs, (), (override));
+  MOCK_METHOD(void, ConfigureGPIOController, (), (override));
+  MOCK_METHOD(void, ConfigureEStop, (), (override));
+
+  MOCK_METHOD(void, DefineRobotDriver, (), (override));
+
+  MOCK_METHOD(void, UpdateHwStates, (), (override));
+  MOCK_METHOD(void, UpdateMotorsStateDataTimedOut, (), (override));
+  MOCK_METHOD(void, UpdateDriverStateMsg, (), (override));
+  MOCK_METHOD(void, UpdateFlagErrors, (), (override));
+  MOCK_METHOD(void, UpdateDriverStateDataTimedOut, (), (override));
+
+  MOCK_METHOD(std::vector<float>, GetSpeedCommands, (), (const, override));
+
+  MOCK_METHOD(void, DiagnoseErrors, (diagnostic_updater::DiagnosticStatusWrapper &), (override));
+  MOCK_METHOD(void, DiagnoseStatus, (diagnostic_updater::DiagnosticStatusWrapper &), (override));
+
+  void DefaultDefineRobotDriver()
+  {
+    robot_driver_ = std::make_shared<panther_hardware_interfaces_test::MockRobotDriver::NiceMock>();
+  }
+
+  void SetHwCommandVelocity(const std::vector<double> & velocity)
+  {
+    hw_commands_velocities_ = velocity;
+  }
+  void SetHwStatePosition(const std::vector<double> & position) { hw_states_positions_ = position; }
+  void SetHwStateVelocity(const std::vector<double> & velocity)
+  {
+    hw_states_velocities_ = velocity;
+  }
+  void SetHwStateEffort(const std::vector<double> & effort) { hw_states_efforts_ = effort; }
+
+  std::shared_ptr<panther_hardware_interfaces_test::MockRobotDriver::NiceMock> GetMockRobotDriver()
+  {
+    return mock_robot_driver_;
+  }
+
+  std::shared_ptr<panther_hardware_interfaces_test::MockGPIOController::NiceMock>
+  GetMockGPIOController()
+  {
+    return mock_gpio_controller_;
+  }
+
+  std::shared_ptr<panther_hardware_interfaces_test::MockEStop::NiceMock> GetMockEStop()
+  {
+    return mock_e_stop_;
+  }
+
+  using NiceMock = testing::NiceMock<MockUGVSystem>;
+
+private:
+  std::shared_ptr<panther_hardware_interfaces_test::MockRobotDriver::NiceMock> mock_robot_driver_;
+  std::shared_ptr<panther_hardware_interfaces_test::MockGPIOController::NiceMock>
+    mock_gpio_controller_;
+  std::shared_ptr<panther_hardware_interfaces_test::MockEStop::NiceMock> mock_e_stop_;
+};
+
+class TestUGVSystem : public ::testing::Test
+{
+public:
+  TestUGVSystem()
+  {
+    ugv_system_ =
+      std::make_shared<MockUGVSystem::NiceMock>(std::vector<std::string>{"fl", "fr", "rl", "rr"});
+
+    hardware_info_ = panther_hardware_interfaces_test::GenerateDefaultHardwareInfo();
+  }
+
+  ~TestUGVSystem() {}
+
+protected:
+  std::shared_ptr<MockUGVSystem::NiceMock> ugv_system_;
+  hardware_interface::HardwareInfo hardware_info_;
+};
+
+TEST_F(TestUGVSystem, OnInit)
+{
+  EXPECT_CALL(*ugv_system_, ReadCANopenSettingsDriverCANIDs()).Times(1);
+
+  auto callback_return = ugv_system_->on_init(hardware_info_);
+
+  EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS);
+}
+
+TEST_F(TestUGVSystem, OnConfigure)
+{
+  ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
+
+  EXPECT_CALL(*ugv_system_, DefineRobotDriver()).WillOnce(::testing::Invoke([&]() {
+    ugv_system_->DefaultDefineRobotDriver();
+  }));
+  EXPECT_CALL(*ugv_system_, ConfigureGPIOController()).Times(1);
+  EXPECT_CALL(*ugv_system_, ConfigureEStop()).Times(1);
+  auto callback_return = ugv_system_->on_configure(rclcpp_lifecycle::State());
+
+  EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS);
+}
+
+TEST_F(TestUGVSystem, OnCleanup)
+{
+  ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
+  ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State()));
+
+  EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Deinitialize()).Times(1);
+  auto callback_return = ugv_system_->on_cleanup(rclcpp_lifecycle::State());
+
+  EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS);
+}
+
+TEST_F(TestUGVSystem, OnActivate)
+{
+  rclcpp::init(0, nullptr);
+
+  ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
+  ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State()));
+
+  EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Activate()).Times(1);
+  EXPECT_CALL(*ugv_system_->GetMockGPIOController(), QueryControlInterfaceIOStates()).Times(1);
+  EXPECT_CALL(*ugv_system_->GetMockEStop(), ReadEStopState()).Times(1);
+  auto callback_return = ugv_system_->on_activate(rclcpp_lifecycle::State());
+
+  EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS);
+
+  rclcpp::shutdown();
+}
+
+TEST_F(TestUGVSystem, OnDeactivate)
+{
+  rclcpp::init(0, nullptr);
+
+  ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
+  ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State()));
+  ASSERT_NO_THROW(ugv_system_->on_activate(rclcpp_lifecycle::State()));
+
+  EXPECT_CALL(*ugv_system_->GetMockEStop(), TriggerEStop()).Times(1);
+  auto callback_return = ugv_system_->on_deactivate(rclcpp_lifecycle::State());
+
+  EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS);
+
+  rclcpp::shutdown();
+}
+
+TEST_F(TestUGVSystem, OnShutdown)
+{
+  ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
+  ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State()));
+
+  EXPECT_CALL(*ugv_system_->GetMockEStop(), TriggerEStop()).Times(1);
+  EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Deinitialize()).Times(1);
+  auto callback_return = ugv_system_->on_shutdown(rclcpp_lifecycle::State());
+
+  EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS);
+}
+
+TEST_F(TestUGVSystem, OnError)
+{
+  ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
+  ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State()));
+
+  EXPECT_CALL(*ugv_system_->GetMockEStop(), TriggerEStop()).Times(1);
+  EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Deinitialize()).Times(1);
+  auto callback_return = ugv_system_->on_error(rclcpp_lifecycle::State());
+
+  EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS);
+}
+
+TEST_F(TestUGVSystem, ExportStateInterfacesInitialValues)
+{
+  std::vector<std::string> prefixes = {"fl", "fr", "rl", "rr"};
+
+  ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
+
+  std::vector<hardware_interface::StateInterface> state_interfaces =
+    ugv_system_->export_state_interfaces();
+
+  ASSERT_EQ(state_interfaces.size(), 12);
+
+  int i = 0;
+  for (const auto & prefix : prefixes) {
+    EXPECT_EQ(
+      state_interfaces[i].get_name(),
+      prefix + "_wheel_joint/" + hardware_interface::HW_IF_POSITION);
+    EXPECT_EQ(
+      state_interfaces[i + 1].get_name(),
+      prefix + "_wheel_joint/" + hardware_interface::HW_IF_VELOCITY);
+    EXPECT_EQ(
+      state_interfaces[i + 2].get_name(),
+      prefix + "_wheel_joint/" + hardware_interface::HW_IF_EFFORT);
+
+    i += 3;
+  }
+
+  auto all_interfaces_are_nan = std::all_of(
+    state_interfaces.begin(), state_interfaces.end(),
+    [](const hardware_interface::StateInterface & state) { return std::isnan(state.get_value()); });
+
+  EXPECT_TRUE(all_interfaces_are_nan);
+}
+
+TEST_F(TestUGVSystem, ExportStateInterfaces)
+{
+  std::vector<double> position = {1.0, 2.0, 3.0, 4.0};
+  std::vector<double> velocity = {5.0, 6.0, 7.0, 8.0};
+  std::vector<double> effort = {9.0, 10.0, 11.0, 12.0};
+
+  ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
+
+  ugv_system_->SetHwStatePosition(position);
+  ugv_system_->SetHwStateVelocity(velocity);
+  ugv_system_->SetHwStateEffort(effort);
+
+  std::vector<hardware_interface::StateInterface> state_interfaces =
+    ugv_system_->export_state_interfaces();
+
+  ASSERT_EQ(state_interfaces.size(), 12);
+
+  for (std::size_t i = 0; i < 4; i++) {
+    EXPECT_FLOAT_EQ(state_interfaces[i * 3].get_value(), position[i]);
+    EXPECT_FLOAT_EQ(state_interfaces[i * 3 + 1].get_value(), velocity[i]);
+    EXPECT_FLOAT_EQ(state_interfaces[i * 3 + 2].get_value(), effort[i]);
+  }
+}
+
+TEST_F(TestUGVSystem, ExportCommandInterfacesInitialValues)
+{
+  std::vector<std::string> prefixes = {"fl", "fr", "rl", "rr"};
+
+  ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
+
+  std::vector<hardware_interface::CommandInterface> command_interfaces =
+    ugv_system_->export_command_interfaces();
+
+  ASSERT_EQ(command_interfaces.size(), 4);
+
+  EXPECT_EQ(
+    command_interfaces[0].get_name(),
+    std::string("fl_wheel_joint/") + hardware_interface::HW_IF_VELOCITY);
+  EXPECT_EQ(
+    command_interfaces[1].get_name(),
+    std::string("fr_wheel_joint/") + hardware_interface::HW_IF_VELOCITY);
+  EXPECT_EQ(
+    command_interfaces[2].get_name(),
+    std::string("rl_wheel_joint/") + hardware_interface::HW_IF_VELOCITY);
+  EXPECT_EQ(
+    command_interfaces[3].get_name(),
+    std::string("rr_wheel_joint/") + hardware_interface::HW_IF_VELOCITY);
+
+  std::for_each(
+    command_interfaces.begin(), command_interfaces.end(),
+    [](const hardware_interface::CommandInterface & command) {
+      EXPECT_FLOAT_EQ(command.get_value(), 0.0);
+    });
+}
+
+TEST_F(TestUGVSystem, ExportCommandInterfaces)
+{
+  std::vector<double> velocity = {1.0, 2.0, 3.0, 4.0};
+
+  ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
+
+  ugv_system_->SetHwCommandVelocity(velocity);
+
+  std::vector<hardware_interface::CommandInterface> command_interfaces =
+    ugv_system_->export_command_interfaces();
+
+  ASSERT_EQ(command_interfaces.size(), 4);
+
+  for (std::size_t i = 0; i < 4; i++) {
+    EXPECT_FLOAT_EQ(command_interfaces[i].get_value(), velocity[i]);
+  }
+}
+
+TEST_F(TestUGVSystem, Read)
+{
+  rclcpp::init(0, nullptr);
+
+  ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
+  ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State()));
+  ASSERT_NO_THROW(ugv_system_->on_activate(rclcpp_lifecycle::State()));
+
+  EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), UpdateMotorsState()).Times(1);
+  EXPECT_CALL(*ugv_system_, UpdateHwStates()).Times(1);
+  EXPECT_CALL(*ugv_system_, UpdateMotorsStateDataTimedOut()).Times(1);
+  EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), UpdateDriversState()).Times(1);
+  EXPECT_CALL(*ugv_system_, UpdateDriverStateMsg()).Times(1);
+  EXPECT_CALL(*ugv_system_, UpdateFlagErrors()).Times(1);
+  EXPECT_CALL(*ugv_system_, UpdateDriverStateDataTimedOut()).Times(1);
+  EXPECT_CALL(*ugv_system_->GetMockEStop(), ReadEStopState()).Times(1);
+
+  auto callback_return = ugv_system_->read(
+    rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration(0, 0));
+
+  EXPECT_EQ(callback_return, hardware_interface::return_type::OK);
+
+  rclcpp::shutdown();
+}
+
+TEST_F(TestUGVSystem, Write)
+{
+  rclcpp::init(0, nullptr);
+
+  const auto velocity = std::vector<float>{1.0, 2.0, 3.0, 4.0};
+
+  ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
+  ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State()));
+  ASSERT_NO_THROW(ugv_system_->on_activate(rclcpp_lifecycle::State()));
+
+  EXPECT_CALL(*ugv_system_, GetSpeedCommands()).WillOnce(::testing::Return(velocity));
+  EXPECT_CALL(*ugv_system_->GetMockEStop(), ReadEStopState()).Times(1);
+  EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), SendSpeedCommands(velocity)).Times(1);
+
+  auto callback_return = ugv_system_->write(
+    rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration(0, 0));
+
+  EXPECT_EQ(callback_return, hardware_interface::return_type::OK);
+
+  rclcpp::shutdown();
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  const auto result = RUN_ALL_TESTS();
+  return result;
+}
diff --git a/panther_hardware_interfaces/test/utils/mock_driver.hpp b/panther_hardware_interfaces/test/utils/mock_driver.hpp
index 7af1cca1..da2b3390 100644
--- a/panther_hardware_interfaces/test/utils/mock_driver.hpp
+++ b/panther_hardware_interfaces/test/utils/mock_driver.hpp
@@ -30,6 +30,15 @@ namespace panther_hardware_interfaces_test
 class MockDriver : public panther_hardware_interfaces::DriverInterface
 {
 public:
+  MockDriver()
+  {
+    ON_CALL(*this, Boot()).WillByDefault(::testing::Invoke([]() {
+      std::promise<void> promise;
+      promise.set_value();
+      return promise.get_future();
+    }));
+  }
+
   MOCK_METHOD(std::future<void>, Boot, (), (override));
   MOCK_METHOD(bool, IsCANError, (), (const, override));
   MOCK_METHOD(bool, IsHeartbeatTimeout, (), (const, override));
@@ -52,6 +61,8 @@ class MockDriver : public panther_hardware_interfaces::DriverInterface
     motor_drivers_.emplace(name, motor_driver);
   }
 
+  using NiceMock = testing::NiceMock<MockDriver>;
+
 private:
   std::map<std::string, std::shared_ptr<panther_hardware_interfaces::MotorDriverInterface>>
     motor_drivers_;
@@ -63,6 +74,8 @@ class MockMotorDriver : public panther_hardware_interfaces::MotorDriverInterface
   MOCK_METHOD(panther_hardware_interfaces::MotorDriverState, ReadState, (), (override));
   MOCK_METHOD(void, SendCmdVel, (const std::int32_t cmd), (override));
   MOCK_METHOD(void, TurnOnSafetyStop, (), (override));
+
+  using NiceMock = testing::NiceMock<MockMotorDriver>;
 };
 
 }  // namespace panther_hardware_interfaces_test
diff --git a/panther_hardware_interfaces/test/utils/system_test_utils.hpp b/panther_hardware_interfaces/test/utils/system_test_utils.hpp
new file mode 100644
index 00000000..db294b4e
--- /dev/null
+++ b/panther_hardware_interfaces/test/utils/system_test_utils.hpp
@@ -0,0 +1,174 @@
+// 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_HARDWARE_INTERFACES_TEST_SYSTEM_TEST_UTILS_HPP_
+#define PANTHER_HARDWARE_INTERFACES_TEST_SYSTEM_TEST_UTILS_HPP_
+
+#include <cstdint>
+#include <future>
+#include <memory>
+#include <string>
+
+#include <gmock/gmock.h>
+
+#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp"
+#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
+#include "panther_hardware_interfaces/panther_system/system_e_stop.hpp"
+
+#include "utils/mock_driver.hpp"
+
+namespace panther_hardware_interfaces_test
+{
+
+class MockRobotDriver : public panther_hardware_interfaces::RobotDriverInterface
+{
+public:
+  MOCK_METHOD(void, Initialize, (), (override));
+  MOCK_METHOD(void, Deinitialize, (), (override));
+  MOCK_METHOD(void, Activate, (), (override));
+  MOCK_METHOD(void, UpdateCommunicationState, (), (override));
+  MOCK_METHOD(void, UpdateMotorsState, (), (override));
+  MOCK_METHOD(void, UpdateDriversState, (), (override));
+  MOCK_METHOD(
+    const panther_hardware_interfaces::DriverData &, GetData, (const std::string &), (override));
+  MOCK_METHOD(void, SendSpeedCommands, (const std::vector<float> &), (override));
+  MOCK_METHOD(void, TurnOnEStop, (), (override));
+  MOCK_METHOD(void, TurnOffEStop, (), (override));
+  MOCK_METHOD(void, AttemptErrorFlagReset, (), (override));
+  MOCK_METHOD(bool, CommunicationError, (), (override));
+
+  using NiceMock = testing::NiceMock<MockRobotDriver>;
+};
+
+class MockGPIODriver : public panther_hardware_interfaces::GPIODriverInterface
+{
+public:
+  MOCK_METHOD(void, GPIOMonitorEnable, (const bool, const unsigned), (override));
+  MOCK_METHOD(
+    void, ConfigureEdgeEventCallback,
+    (const std::function<void(const panther_hardware_interfaces::GPIOInfo &)> &), (override));
+  MOCK_METHOD(
+    void, ChangePinDirection,
+    (const panther_hardware_interfaces::GPIOPin, const gpiod::line::direction), (override));
+  MOCK_METHOD(
+    bool, IsPinAvailable, (const panther_hardware_interfaces::GPIOPin), (const, override));
+  MOCK_METHOD(bool, IsPinActive, (const panther_hardware_interfaces::GPIOPin), (override));
+  MOCK_METHOD(
+    bool, SetPinValue, (const panther_hardware_interfaces::GPIOPin, const bool), (override));
+
+  using NiceMock = testing::NiceMock<MockGPIODriver>;
+};
+
+class MockGPIOController : public panther_hardware_interfaces::GPIOControllerInterface
+{
+public:
+  MockGPIOController() : GPIOControllerInterface()
+  {
+    gpio_driver_ = std::make_shared<::testing::NiceMock<MockGPIODriver>>();
+  }
+
+  MOCK_METHOD(void, Start, (), (override));
+  MOCK_METHOD(void, EStopTrigger, (), (override));
+  MOCK_METHOD(void, EStopReset, (), (override));
+  MOCK_METHOD(bool, MotorPowerEnable, (const bool), (override));
+  MOCK_METHOD(bool, FanEnable, (const bool), (override));
+  MOCK_METHOD(bool, AUXPowerEnable, (const bool), (override));
+  MOCK_METHOD(bool, DigitalPowerEnable, (const bool), (override));
+  MOCK_METHOD(bool, ChargerEnable, (const bool), (override));
+  MOCK_METHOD(bool, LEDControlEnable, (const bool), (override));
+  MOCK_METHOD(
+    (std::unordered_map<panther_hardware_interfaces::GPIOPin, bool>), QueryControlInterfaceIOStates,
+    (), (const, override));
+
+  using NiceMock = testing::NiceMock<MockGPIOController>;
+};
+
+class MockEStop : public panther_hardware_interfaces::EStopInterface
+{
+public:
+  MockEStop() : EStopInterface() {}
+
+  MOCK_METHOD(bool, ReadEStopState, (), (override));
+  MOCK_METHOD(void, TriggerEStop, (), (override));
+  MOCK_METHOD(void, ResetEStop, (), (override));
+
+  using NiceMock = testing::NiceMock<MockEStop>;
+};
+
+hardware_interface::HardwareInfo GenerateDefaultHardwareInfo()
+{
+  hardware_interface::HardwareInfo hardware_info;
+  hardware_info.name = "test";
+  hardware_info.hardware_class_type = "UGVSystem";
+
+  hardware_interface::InterfaceInfo vel_command_interface;
+  vel_command_interface.name = "velocity";
+  hardware_interface::InterfaceInfo pos_state_interface;
+  pos_state_interface.name = "position";
+  hardware_interface::InterfaceInfo vel_state_interface;
+  vel_state_interface.name = "velocity";
+  hardware_interface::InterfaceInfo eff_state_interface;
+  eff_state_interface.name = "effort";
+
+  hardware_interface::ComponentInfo wheel_joint;
+  wheel_joint.command_interfaces = {vel_command_interface};
+  wheel_joint.state_interfaces = {pos_state_interface, vel_state_interface, eff_state_interface};
+
+  auto fl_wheel_joint = wheel_joint;
+  fl_wheel_joint.name = "fl_wheel_joint";
+  auto fr_wheel_joint = wheel_joint;
+  fr_wheel_joint.name = "fr_wheel_joint";
+  auto rl_wheel_joint = wheel_joint;
+  rl_wheel_joint.name = "rl_wheel_joint";
+  auto rr_wheel_joint = wheel_joint;
+  rr_wheel_joint.name = "rr_wheel_joint";
+
+  hardware_info.joints = {fl_wheel_joint, fr_wheel_joint, rl_wheel_joint, rr_wheel_joint};
+
+  std::unordered_map<std::string, std::string> hardware_paremeters = {
+    // drivetrain settings
+    {"motor_torque_constant", "0.11"},
+    {"gear_ratio", "30.08"},
+    {"gearbox_efficiency", "0.75"},
+    {"encoder_resolution", "1600"},
+    {"max_rpm_motor_speed", "3600.0"},
+
+    // CANopen settings
+    {"can_interface_name", "panther_can"},
+    {"master_can_id", "3"},
+    {"pdo_motor_states_timeout_ms", "15"},
+    {"pdo_driver_state_timeout_ms", "75"},
+    {"sdo_operation_timeout_ms", "100"},
+
+    // Driver states update frequency
+    {"driver_states_update_frequency", "20.0"},
+
+    // Roboteq initialization and activation attempts
+    {"max_roboteq_initialization_attempts", "1"},
+    {"max_roboteq_activation_attempts", "1"},
+
+    // Roboteq error filter params
+    {"max_write_pdo_cmds_errors_count", "1"},
+    {"max_read_pdo_motor_states_errors_count", "1"},
+    {"max_read_pdo_driver_state_errors_count", "1"},
+  };
+
+  hardware_info.hardware_parameters = hardware_paremeters;
+
+  return hardware_info;
+}
+
+}  // namespace panther_hardware_interfaces_test
+
+#endif  // PANTHER_HARDWARE_INTERFACES_TEST_SYSTEM_TEST_UTILS_HPP_

From 0fd544a50cf5574c8c83a25f1c539c44fe4163e9 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Tue, 17 Sep 2024 17:36:48 +0200
Subject: [PATCH 048/100] Update readme

---
 README.md                                |  5 ++--
 panther_utils/panther_utils/arguments.py | 31 ++++--------------------
 2 files changed, 8 insertions(+), 28 deletions(-)

diff --git a/README.md b/README.md
index a120c02a..1382302f 100644
--- a/README.md
+++ b/README.md
@@ -101,14 +101,15 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | 🤖🖥️  | `localization_mode`          | Specifies the localization mode:  <br/>- 'relative' `odometry/filtered` data is relative to the initial position and orientation. <br/>- 'enu' `odometry/filtered` data is relative to initial position and ENU (East North Up) orientation. <br/> ***string:*** `relative` (choices: `relative`, `enu`)                                                    |
 | 🤖🖥️  | `namespace`                  | Add namespace to all launched nodes. <br/> ***string:*** `env(ROBOT_NAMESPACE)`                                                                                                                                                                                                                                                                               |
 | 🤖🖥️  | `publish_robot_state`        | Whether to publish the default Panther robot description. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                |
-| 🖥️   | `robots`                     | The list of the robots spawned in the simulation e.g. `robots:='robot1={x: 1.0, y: -2.0}; robot2={x: 1.0, y: -4.0}'` <br/> ***string:*** `''`                                                                                                                                                                                                                  |
+| 🖥️   | `robot_configuration`         | Path to robot configuration YAML file. <br/> ***string:*** [`configuration.yaml`](panther_gazebo/config/configuration.yaml)                                                                                              |
+| 🖥️   | `robot_model`                 | Specify robot model type. <br/> ***string:*** `env(ROBOT_MODEL)` if not specified `panther` (choices: `lynx`, `panther`)                                                                                                                                                                                                                  |
 | 🤖🖥️  | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`PantherSafetyBT.btproj`](./panther_manager/behavior_trees/PantherSafetyBT.btproj)                                                                                                                                                                   |
 | 🤖🖥️  | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts_config.yaml`](./panther_manager/config/shutdown_hosts_config.yaml)                                                                                                                                                                                                               |
 | 🤖🖥️  | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                                   |
 | 🤖🖥️  | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                                             |
 | 🤖🖥️  | `user_led_animations_file`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                                                             |
 | 🤖🖥️  | `wheel_config_path`          | Path to wheel configuration file. <br/> ***string:*** [`{wheel_type}.yaml`](./panther_description/config)                                                                                                                                                                                                                                                   |
-| 🤖🖥️  | `wheel_type`                 | Specify the wheel type. If the selected wheel type is not 'custom', the wheel_config_path and controller_config_path arguments will be automatically adjusted and can be omitted. <br/> ***string:*** `WH01` (choices: `WH01`, `WH02`, `WH04`, `custom`) |
+| 🤖🖥️  | `wheel_type`                 | Specify the wheel type. If the selected wheel type is not 'custom', the wheel_config_path and controller_config_path arguments will be automatically adjusted and can be omitted. <br/> ***string:*** `WH01` (choices: `WH01`, `WH02`, `WH04`, `WH05`, `custom`) |
 | 🖥️   | `x`                          | Initial robot position in the global 'x' axis. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                          |
 | 🖥️   | `y`                          | Initial robot position in the global 'y' axis. <br/> ***float:***` -2.0`                                                                                                                                                                                                                                                                                         |
 | 🖥️   | `z`                          | Initial robot position in the global 'z' axis. <br/> ***float:*** `0.2`                                                                                                                                                                                                                                                                                          |
diff --git a/panther_utils/panther_utils/arguments.py b/panther_utils/panther_utils/arguments.py
index 1cea5d56..ad9a7f2e 100644
--- a/panther_utils/panther_utils/arguments.py
+++ b/panther_utils/panther_utils/arguments.py
@@ -15,12 +15,11 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from typing import List, Text, Tuple
+from typing import Text, Tuple
 
 import yaml
 from launch.action import Action
 from launch.actions import DeclareLaunchArgument
-from launch.frontend import Entity, Parser
 from launch.launch_context import LaunchContext
 from launch.substitutions import EnvironmentVariable, LaunchConfiguration
 from launch.utilities import normalize_to_list_of_substitutions, perform_substitutions
@@ -40,24 +39,6 @@ def __init__(self, path: LaunchConfiguration, **kwargs) -> None:
         super().__init__(**kwargs)
         self.__path = normalize_to_list_of_substitutions(path)
 
-    @classmethod
-    def parse(cls, entity: Entity, parser: "Parser"):
-        """Parse `arg` tag."""
-        _, kwargs = super().parse(entity, parser)
-        kwargs["name"] = parser.escape_characters(entity.get_attr("name"))
-        default_value = entity.get_attr("default", optional=True)
-        if default_value is not None:
-            kwargs["default_value"] = parser.parse_substitution(default_value)
-        description = entity.get_attr("description", optional=True)
-        if description is not None:
-            kwargs["description"] = parser.escape_characters(description)
-        choices = entity.get_attr("choice", data_type=List[Entity], optional=True)
-        if choices is not None:
-            kwargs["choices"] = [
-                parser.escape_characters(choice.get_attr("value")) for choice in choices
-            ]
-        return cls, kwargs
-
     @property
     def path(self) -> Text:
         """Getter for self.__path."""
@@ -68,7 +49,7 @@ def execute(self, context: LaunchContext):
         path = perform_substitutions(context, self.path)
         yaml_data = self.load_yaml_file(path)
         yaml_data = self.normalize_robot_configuration(yaml_data)
-        namespace, robot_config = self.extract_single_robot_configuration(yaml_data)
+        namespace, robot_config = self.extract_first_robot_configuration(yaml_data)
         list_of_args = self.create_launch_arguments(namespace, robot_config)
         for arg in list_of_args:
             arg.execute(context)
@@ -124,12 +105,10 @@ def normalize_robot_configuration(self, yaml_data: dict) -> dict:
 
         raise ValueError("Invalid YAML structure: The data does not match expected formats.")
 
-    def extract_single_robot_configuration(
-        self, yaml_data: dict, idx: int = 0
-    ) -> Tuple[str, dict]:
-        """Extracts the namespace and configuration based on the provided index from a YAML dictionary."""
+    def extract_first_robot_configuration(self, yaml_data: dict) -> Tuple[str, dict]:
+        """Extracts the namespace and configuration of first element in dict."""
         keys = list(yaml_data.keys())
-        namespace = keys[idx]
+        namespace = keys[0]
         configuration = yaml_data[namespace]
         return namespace, configuration
 

From 511214e15a1932e5c98621eba573ee2c42ae178d Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Tue, 17 Sep 2024 18:36:12 +0200
Subject: [PATCH 049/100] Add add_world_transform arg

---
 .../launch/simulate_robot.launch.py           | 37 +++++++++++++++++++
 panther_gazebo/launch/spawn_robot.launch.py   |  2 +
 panther_utils/panther_utils/arguments.py      |  4 +-
 3 files changed, 41 insertions(+), 2 deletions(-)

diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py
index 4c271a8e..34afceca 100644
--- a/panther_gazebo/launch/simulate_robot.launch.py
+++ b/panther_gazebo/launch/simulate_robot.launch.py
@@ -31,12 +31,24 @@
 
 def generate_launch_description():
 
+    add_world_transform = LaunchConfiguration("add_world_transform")
     components_config_path = LaunchConfiguration("components_config_path")
     gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path")
     namespace = LaunchConfiguration("namespace")
     robot_configuration = LaunchConfiguration("robot_configuration")
     robot_model = LaunchConfiguration("robot_model")
     use_ekf = LaunchConfiguration("use_ekf")
+    wheel_type = LaunchConfiguration("wheel_type")
+
+    declare_add_world_transform_arg = DeclareLaunchArgument(
+        "add_world_transform",
+        default_value="False",
+        description=(
+            "Adds a world frame that connects the tf trees of individual robots (useful when running"
+            " multiple robots)."
+        ),
+        choices=["True", "true", "False", "false"],
+    )
 
     declare_battery_config_path_arg = DeclareLaunchArgument(
         "battery_config_path",
@@ -96,6 +108,7 @@ def generate_launch_description():
             "namespace": namespace,
             "robot_model": robot_model,
             "use_sim": "True",
+            "wheel_type": wheel_type,
         }.items(),
     )
 
@@ -141,6 +154,7 @@ def generate_launch_description():
             "namespace": namespace,
             "publish_robot_state": "False",
             "use_sim": "True",
+            "wheel_type": wheel_type,
         }.items(),
     )
 
@@ -191,10 +205,32 @@ def generate_launch_description():
         emulate_tty=True,
     )
 
+    child_tf = PythonExpression(["'", namespace, "' + '/odom' if '", namespace, "' else 'odom'"])
+
+    world_transform = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_tf_publisher",
+        arguments=[
+            LaunchConfiguration("x"),
+            LaunchConfiguration("y"),
+            LaunchConfiguration("z"),
+            LaunchConfiguration("roll"),
+            LaunchConfiguration("pitch"),
+            LaunchConfiguration("yaw"),
+            "world",
+            child_tf,
+        ],
+        namespace=namespace,
+        emulate_tty=True,
+        condition=IfCondition(add_world_transform),
+    )
+
     return LaunchDescription(
         [
             declare_robot_configuration_arg,
             DeclareRobotArgs(robot_configuration),
+            declare_add_world_transform_arg,
             declare_battery_config_path_arg,
             declare_components_config_path_arg,
             declare_gz_bridge_config_path_arg,
@@ -207,5 +243,6 @@ def generate_launch_description():
             ekf_launch,
             simulate_components,
             gz_bridge,
+            world_transform,
         ]
     )
diff --git a/panther_gazebo/launch/spawn_robot.launch.py b/panther_gazebo/launch/spawn_robot.launch.py
index 5a54c369..173f3e60 100644
--- a/panther_gazebo/launch/spawn_robot.launch.py
+++ b/panther_gazebo/launch/spawn_robot.launch.py
@@ -39,6 +39,7 @@ def generate_launch_description():
     roll = LaunchConfiguration("roll")
     pitch = LaunchConfiguration("pitch")
     yaw = LaunchConfiguration("yaw")
+    wheel_type = LaunchConfiguration("wheel_type")
 
     declare_robot_configuration_arg = DeclareLaunchArgument(
         "robot_configuration",
@@ -67,6 +68,7 @@ def generate_launch_description():
             "add_wheel_joints": add_wheel_joints,
             "namespace": namespace,
             "robot_model": robot_model,
+            "wheel_type": wheel_type,
             "use_sim": "True",
         }.items(),
     )
diff --git a/panther_utils/panther_utils/arguments.py b/panther_utils/panther_utils/arguments.py
index ad9a7f2e..05eb4622 100644
--- a/panther_utils/panther_utils/arguments.py
+++ b/panther_utils/panther_utils/arguments.py
@@ -34,10 +34,10 @@
 class DeclareRobotArgs(Action):
     """Retrieves and validate the robot configuration from the YAML data."""
 
-    def __init__(self, path: LaunchConfiguration, **kwargs) -> None:
+    def __init__(self, robot_configuration_path: LaunchConfiguration, **kwargs) -> None:
         """Create a DeclareRobotArgs action."""
         super().__init__(**kwargs)
-        self.__path = normalize_to_list_of_substitutions(path)
+        self.__path = normalize_to_list_of_substitutions(robot_configuration_path)
 
     @property
     def path(self) -> Text:

From 2e481d081b5059ac211c42c311523ab6d1465fe8 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Wed, 18 Sep 2024 14:54:14 +0200
Subject: [PATCH 050/100] Remove yaml with arg

---
 lynx_description/launch/load_urdf.launch.py   |   9 +-
 panther_bringup/launch/bringup.launch.py      |  11 +-
 .../launch/controller.launch.py               |   4 +-
 .../launch/load_urdf.launch.py                |   2 +-
 panther_gazebo/config/configuration.yaml      |  14 --
 .../launch/simulate_robot.launch.py           |  94 +++++-----
 panther_gazebo/launch/simulation.launch.py    |  42 ++---
 panther_gazebo/launch/spawn_robot.launch.py   |  83 ++++++---
 panther_utils/panther_utils/arguments.py      | 175 ------------------
 9 files changed, 143 insertions(+), 291 deletions(-)
 delete mode 100644 panther_gazebo/config/configuration.yaml
 delete mode 100644 panther_utils/panther_utils/arguments.py

diff --git a/lynx_description/launch/load_urdf.launch.py b/lynx_description/launch/load_urdf.launch.py
index 9823d514..d4b218b4 100644
--- a/lynx_description/launch/load_urdf.launch.py
+++ b/lynx_description/launch/load_urdf.launch.py
@@ -99,6 +99,7 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
+    wheel_config_path = LaunchConfiguration("wheel_config_path")
     declare_wheel_config_path_arg = DeclareLaunchArgument(
         "wheel_config_path",
         default_value=PathJoinSubstitution(
@@ -115,15 +116,13 @@ def generate_launch_description():
         ),
     )
 
-    wheel_config_path = LaunchConfiguration("wheel_config_path")
     declare_wheel_type_arg = DeclareLaunchArgument(
         "wheel_type",
         default_value="WH05",
         description=(
-            "Type of wheel. If you choose a value from the preset options ('WH05'), you can "
-            "ignore the 'wheel_config_path' and 'controller_config_path' parameters. "
-            "For custom wheels, please define these parameters to point to files that "
-            "accurately describe the custom wheels."
+            "Specify the wheel type. If the selected wheel type is not 'custom', "
+            "the 'wheel_config_path' and 'controller_config_path' arguments will be "
+            "automatically adjusted and can be omitted."
         ),
         choices=["WH05", "custom"],
     )
diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py
index 716c9c9c..37886eb7 100644
--- a/panther_bringup/launch/bringup.launch.py
+++ b/panther_bringup/launch/bringup.launch.py
@@ -14,6 +14,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
+
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction
 from launch.conditions import IfCondition, UnlessCondition
@@ -51,10 +53,11 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
-    robot_model = EnvironmentVariable("ROBOT_MODEL", default_value="panther")
-    serial_no = EnvironmentVariable(name="PANTHER_SERIAL_NO", default_value="----")
-    panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0")
-    welcome_info = welcome_msg(robot_model, serial_no, panther_version)
+    robot_model = os.environ.get("ROBOT_MODEL", default="PTH")
+    robot_model = "lynx" if robot_model == "LNX" else "panther"
+    robot_serial_no = EnvironmentVariable(name="ROBOT_SERIAL_NO", default_value="----")
+    robot_version = EnvironmentVariable(name="ROBOT_VERSION", default_value="1.0")
+    welcome_info = welcome_msg(robot_model, robot_serial_no, robot_version)
 
     controller_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py
index 14e3f7c1..880ceabb 100644
--- a/panther_controller/launch/controller.launch.py
+++ b/panther_controller/launch/controller.launch.py
@@ -96,7 +96,7 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
-    wheel_config_path = LaunchConfiguration("wheel_config_path")
+    use_sim = LaunchConfiguration("use_sim")
     declare_use_sim_arg = DeclareLaunchArgument(
         "use_sim",
         default_value="False",
@@ -104,6 +104,7 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
+    wheel_config_path = LaunchConfiguration("wheel_config_path")
     declare_wheel_config_path_arg = DeclareLaunchArgument(
         "wheel_config_path",
         default_value=PathJoinSubstitution(
@@ -120,7 +121,6 @@ def generate_launch_description():
         ),
     )
 
-    use_sim = LaunchConfiguration("use_sim")
     declare_wheel_type_arg = DeclareLaunchArgument(
         "wheel_type",
         default_value="WH01",
diff --git a/panther_description/launch/load_urdf.launch.py b/panther_description/launch/load_urdf.launch.py
index fbdd9aee..930c4a83 100644
--- a/panther_description/launch/load_urdf.launch.py
+++ b/panther_description/launch/load_urdf.launch.py
@@ -98,6 +98,7 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
+    wheel_config_path = LaunchConfiguration("wheel_config_path")
     declare_wheel_config_path_arg = DeclareLaunchArgument(
         "wheel_config_path",
         default_value=PathJoinSubstitution(
@@ -114,7 +115,6 @@ def generate_launch_description():
         ),
     )
 
-    wheel_config_path = LaunchConfiguration("wheel_config_path")
     declare_wheel_type_arg = DeclareLaunchArgument(
         "wheel_type",
         default_value="WH01",
diff --git a/panther_gazebo/config/configuration.yaml b/panther_gazebo/config/configuration.yaml
deleted file mode 100644
index 7f817003..00000000
--- a/panther_gazebo/config/configuration.yaml
+++ /dev/null
@@ -1,14 +0,0 @@
-# The minimal configuration file with the default values.
-# namespace: (optional - without this element namespace will not be provided)
-#   robot_model: panther
-#   init_pose: [0.0, 0.0, 0.0]
-#   init_rotation: [0.0, 0.0, 0.0]
-#   configuration:
-#     wheel_type: WH01
-
-panther:
-  robot_model: panther
-  init_pose: [0.0, -2.0, 0.0]
-  init_rotation: [0.0, 0.0, 0.0]
-  configuration:
-    wheel_type: WH01
diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py
index 34afceca..6720b18e 100644
--- a/panther_gazebo/launch/simulate_robot.launch.py
+++ b/panther_gazebo/launch/simulate_robot.launch.py
@@ -14,6 +14,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
+
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.conditions import IfCondition, UnlessCondition
@@ -26,20 +28,13 @@
 from launch_ros.actions import Node, SetUseSimTime
 from launch_ros.substitutions import FindPackageShare
 from nav2_common.launch import ReplaceString
-from panther_utils.arguments import DeclareRobotArgs
 
 
 def generate_launch_description():
 
-    add_world_transform = LaunchConfiguration("add_world_transform")
-    components_config_path = LaunchConfiguration("components_config_path")
-    gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path")
     namespace = LaunchConfiguration("namespace")
-    robot_configuration = LaunchConfiguration("robot_configuration")
-    robot_model = LaunchConfiguration("robot_model")
-    use_ekf = LaunchConfiguration("use_ekf")
-    wheel_type = LaunchConfiguration("wheel_type")
 
+    add_world_transform = LaunchConfiguration("add_world_transform")
     declare_add_world_transform_arg = DeclareLaunchArgument(
         "add_world_transform",
         default_value="False",
@@ -74,6 +69,7 @@ def generate_launch_description():
         ),
     )
 
+    gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path")
     declare_gz_bridge_config_path_arg = DeclareLaunchArgument(
         "gz_bridge_config_path",
         default_value=PathJoinSubstitution(
@@ -82,14 +78,17 @@ def generate_launch_description():
         description="Path to the parameter_bridge configuration file.",
     )
 
-    declare_robot_configuration_arg = DeclareLaunchArgument(
-        "robot_configuration",
-        default_value=PathJoinSubstitution(
-            [FindPackageShare("panther_gazebo"), "config", "configuration.yaml"]
-        ),
-        description="Path to robot configuration YAML file.",
+    robot_model = LaunchConfiguration("robot_model")
+    robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
+    robot_model_env = "lynx" if robot_model_env == "LNX" else "panther"
+    declare_robot_model_arg = DeclareLaunchArgument(
+        "robot_model",
+        default_value=robot_model_env,
+        description="Specify robot model",
+        choices=["lynx", "panther"],
     )
 
+    use_ekf = LaunchConfiguration("use_ekf")
     declare_use_ekf_arg = DeclareLaunchArgument(
         "use_ekf",
         default_value="True",
@@ -97,6 +96,17 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
+    declare_wheel_type_arg = DeclareLaunchArgument(
+        "wheel_type",
+        default_value=PythonExpression(["'WH01' if '", robot_model, "' == 'panther' else 'WH05'"]),
+        description=(
+            "Specify the wheel type. If the selected wheel type is not 'custom', "
+            "the 'wheel_config_path' and 'controller_config_path' arguments will be "
+            "automatically adjusted and can be omitted."
+        ),
+        choices=["WH01", "WH02", "WH04", "WH05", "custom"],
+    )
+
     spawn_robot_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -105,10 +115,6 @@ def generate_launch_description():
         ),
         launch_arguments={
             "add_wheel_joints": "False",
-            "namespace": namespace,
-            "robot_model": robot_model,
-            "use_sim": "True",
-            "wheel_type": wheel_type,
         }.items(),
     )
 
@@ -151,10 +157,8 @@ def generate_launch_description():
             )
         ),
         launch_arguments={
-            "namespace": namespace,
             "publish_robot_state": "False",
             "use_sim": "True",
-            "wheel_type": wheel_type,
         }.items(),
     )
 
@@ -183,8 +187,6 @@ def generate_launch_description():
             )
         ),
         launch_arguments={
-            "components_config_path": components_config_path,
-            "namespace": namespace,
             "use_sim": "True",
         }.items(),
     )
@@ -212,13 +214,21 @@ def generate_launch_description():
         executable="static_transform_publisher",
         name="static_tf_publisher",
         arguments=[
+            "--x",
             LaunchConfiguration("x"),
+            "--y",
             LaunchConfiguration("y"),
+            "--z",
             LaunchConfiguration("z"),
+            "--roll",
             LaunchConfiguration("roll"),
+            "--pitch",
             LaunchConfiguration("pitch"),
+            "--yaw",
             LaunchConfiguration("yaw"),
+            "--frame-id",
             "world",
+            "--child-frame-id",
             child_tf,
         ],
         namespace=namespace,
@@ -226,23 +236,23 @@ def generate_launch_description():
         condition=IfCondition(add_world_transform),
     )
 
-    return LaunchDescription(
-        [
-            declare_robot_configuration_arg,
-            DeclareRobotArgs(robot_configuration),
-            declare_add_world_transform_arg,
-            declare_battery_config_path_arg,
-            declare_components_config_path_arg,
-            declare_gz_bridge_config_path_arg,
-            declare_use_ekf_arg,
-            SetUseSimTime(True),
-            spawn_robot_launch,
-            lights_launch,
-            manager_launch,
-            controller_launch,
-            ekf_launch,
-            simulate_components,
-            gz_bridge,
-            world_transform,
-        ]
-    )
+    actions = [
+        declare_add_world_transform_arg,
+        declare_battery_config_path_arg,
+        declare_components_config_path_arg,
+        declare_gz_bridge_config_path_arg,
+        declare_robot_model_arg,
+        declare_use_ekf_arg,
+        declare_wheel_type_arg,
+        SetUseSimTime(True),
+        spawn_robot_launch,
+        lights_launch,
+        manager_launch,
+        controller_launch,
+        ekf_launch,
+        simulate_components,
+        gz_bridge,
+        world_transform,
+    ]
+
+    return LaunchDescription(actions)
diff --git a/panther_gazebo/launch/simulation.launch.py b/panther_gazebo/launch/simulation.launch.py
index 548100e0..5ea18720 100644
--- a/panther_gazebo/launch/simulation.launch.py
+++ b/panther_gazebo/launch/simulation.launch.py
@@ -17,19 +17,19 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import (
+    EnvironmentVariable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
 from launch_ros.actions import SetUseSimTime
 from launch_ros.substitutions import FindPackageShare
 from nav2_common.launch import ReplaceString
-from panther_utils.arguments import DeclareRobotArgs
 
 
 def generate_launch_description():
 
     gz_gui = LaunchConfiguration("gz_gui")
-    namespace = LaunchConfiguration("namespace")
-    robot_configuration = LaunchConfiguration("robot_configuration")
-
     declare_gz_gui = DeclareLaunchArgument(
         "gz_gui",
         default_value=PathJoinSubstitution(
@@ -38,12 +38,11 @@ def generate_launch_description():
         description="Run simulation with specific GUI layout.",
     )
 
-    declare_robot_configuration_arg = DeclareLaunchArgument(
-        "robot_configuration",
-        default_value=PathJoinSubstitution(
-            [FindPackageShare("panther_gazebo"), "config", "configuration.yaml"]
-        ),
-        description="Path to robot configuration YAML file.",
+    namespace = LaunchConfiguration("namespace")
+    declare_namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Add namespace to all launched nodes.",
     )
 
     namespaced_gz_gui = ReplaceString(
@@ -72,14 +71,13 @@ def generate_launch_description():
         ),
     )
 
-    return LaunchDescription(
-        [
-            declare_gz_gui,
-            declare_robot_configuration_arg,
-            DeclareRobotArgs(robot_configuration),
-            # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo)
-            SetUseSimTime(True),
-            gz_sim,
-            simulate_robots,
-        ]
-    )
+    actions = [
+        declare_gz_gui,
+        declare_namespace_arg,
+        # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo)
+        SetUseSimTime(True),
+        gz_sim,
+        simulate_robots,
+    ]
+
+    return LaunchDescription(actions)
diff --git a/panther_gazebo/launch/spawn_robot.launch.py b/panther_gazebo/launch/spawn_robot.launch.py
index 173f3e60..4f3694ac 100644
--- a/panther_gazebo/launch/spawn_robot.launch.py
+++ b/panther_gazebo/launch/spawn_robot.launch.py
@@ -14,39 +14,69 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
+
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
+    EnvironmentVariable,
     LaunchConfiguration,
     PathJoinSubstitution,
     PythonExpression,
 )
 from launch_ros.actions import Node, SetUseSimTime
 from launch_ros.substitutions import FindPackageShare
-from panther_utils.arguments import DeclareRobotArgs
 from panther_utils.messages import welcome_msg
 
 
 def generate_launch_description():
 
     namespace = LaunchConfiguration("namespace")
-    robot_configuration = LaunchConfiguration("robot_configuration")
+    declare_namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Add namespace to all launched nodes.",
+    )
+
     robot_model = LaunchConfiguration("robot_model")
+    robot_model_env = os.environ.get("ROBOT_MODEL_env", default="PTH")
+    robot_model_env = "lynx" if robot_model_env == "LNX" else "panther"
+    declare_robot_model_arg = DeclareLaunchArgument(
+        "robot_model",
+        default_value=robot_model_env,
+        description="Specify robot model",
+        choices=["lynx", "panther"],
+    )
+
     x = LaunchConfiguration("x")
+    declare_x_arg = DeclareLaunchArgument(
+        "x", default_value="0.0", description="Initial robot position in the global 'x' axis."
+    )
+
     y = LaunchConfiguration("y")
+    declare_y_arg = DeclareLaunchArgument(
+        "y", default_value="-2.0", description="Initial robot position in the global 'y' axis."
+    )
+
     z = LaunchConfiguration("z")
+    declare_z_arg = DeclareLaunchArgument(
+        "z", default_value="0.2", description="Initial robot position in the global 'z' axis."
+    )
+
     roll = LaunchConfiguration("roll")
+    declare_roll_arg = DeclareLaunchArgument(
+        "roll", default_value="0.0", description="Initial robot 'roll' orientation."
+    )
+
     pitch = LaunchConfiguration("pitch")
-    yaw = LaunchConfiguration("yaw")
-    wheel_type = LaunchConfiguration("wheel_type")
+    declare_pitch_arg = DeclareLaunchArgument(
+        "pitch", default_value="0.0", description="Initial robot 'pitch' orientation."
+    )
 
-    declare_robot_configuration_arg = DeclareLaunchArgument(
-        "robot_configuration",
-        default_value=PathJoinSubstitution(
-            [FindPackageShare("panther_gazebo"), "config", "configuration.yaml"]
-        ),
-        description="Path to robot configuration YAML file.",
+    yaw = LaunchConfiguration("yaw")
+    declare_yaw_arg = DeclareLaunchArgument(
+        "yaw", default_value="0.0", description="Initial robot 'yaw' orientation."
     )
 
     log_stats = {
@@ -56,8 +86,6 @@ def generate_launch_description():
     welcome_info = welcome_msg(robot_model, "----", "simulation", log_stats)
 
     urdf_packages = PythonExpression(["'", robot_model, "_description'"])
-    add_wheel_joints = LaunchConfiguration("add_wheel_joints", default="True")
-
     load_urdf = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -65,10 +93,7 @@ def generate_launch_description():
             )
         ),
         launch_arguments={
-            "add_wheel_joints": add_wheel_joints,
-            "namespace": namespace,
-            "robot_model": robot_model,
-            "wheel_type": wheel_type,
+            "add_wheel_joints": LaunchConfiguration("add_wheel_joints", default="True"),
             "use_sim": "True",
         }.items(),
     )
@@ -98,13 +123,19 @@ def generate_launch_description():
         emulate_tty=True,
     )
 
-    return LaunchDescription(
-        [
-            declare_robot_configuration_arg,
-            DeclareRobotArgs(robot_configuration),
-            SetUseSimTime(True),
-            welcome_info,
-            load_urdf,
-            spawn_robot,
-        ]
-    )
+    actions = [
+        declare_namespace_arg,
+        declare_robot_model_arg,
+        declare_x_arg,
+        declare_y_arg,
+        declare_z_arg,
+        declare_roll_arg,
+        declare_pitch_arg,
+        declare_yaw_arg,
+        SetUseSimTime(True),
+        welcome_info,
+        load_urdf,
+        spawn_robot,
+    ]
+
+    return LaunchDescription(actions)
diff --git a/panther_utils/panther_utils/arguments.py b/panther_utils/panther_utils/arguments.py
deleted file mode 100644
index 05eb4622..00000000
--- a/panther_utils/panther_utils/arguments.py
+++ /dev/null
@@ -1,175 +0,0 @@
-#!/usr/bin/env python3
-
-# Copyright 2018 Open Source Robotics Foundation, Inc.
-# 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.
-
-from typing import Text, Tuple
-
-import yaml
-from launch.action import Action
-from launch.actions import DeclareLaunchArgument
-from launch.launch_context import LaunchContext
-from launch.substitutions import EnvironmentVariable, LaunchConfiguration
-from launch.utilities import normalize_to_list_of_substitutions, perform_substitutions
-
-# Define valid configurations for each robot model
-VALID_CONFIGURATIONS = {
-    "panther": {"wheel_type": ["WH01", "WH02", "WH04"]},
-    "lynx": {"wheel_type": ["WH05"]},
-}
-
-
-class DeclareRobotArgs(Action):
-    """Retrieves and validate the robot configuration from the YAML data."""
-
-    def __init__(self, robot_configuration_path: LaunchConfiguration, **kwargs) -> None:
-        """Create a DeclareRobotArgs action."""
-        super().__init__(**kwargs)
-        self.__path = normalize_to_list_of_substitutions(robot_configuration_path)
-
-    @property
-    def path(self) -> Text:
-        """Getter for self.__path."""
-        return self.__path
-
-    def execute(self, context: LaunchContext):
-        """Execute the action."""
-        path = perform_substitutions(context, self.path)
-        yaml_data = self.load_yaml_file(path)
-        yaml_data = self.normalize_robot_configuration(yaml_data)
-        namespace, robot_config = self.extract_first_robot_configuration(yaml_data)
-        list_of_args = self.create_launch_arguments(namespace, robot_config)
-        for arg in list_of_args:
-            arg.execute(context)
-
-    def load_yaml_file(self, path: str) -> dict:
-        """Load YAML file and return its contents."""
-        try:
-            with open(path, "r") as file:
-                return yaml.safe_load(file)
-        except yaml.YAMLError as exc:
-            raise ValueError(f"Error reading YAML file: {exc}") from exc
-
-    def validate_robot_configuration(self, yaml_data: dict) -> None:
-        """Validate the robot model and wheel type configuration."""
-        robot_model = yaml_data.get("robot_model", "")
-        configuration_data = yaml_data.get("configuration", {})
-        wheel_type = configuration_data.get("wheel_type", "")
-
-        if robot_model not in VALID_CONFIGURATIONS:
-            raise ValueError(
-                f"Invalid robot model '{robot_model}'. "
-                f"Valid models are: {', '.join(VALID_CONFIGURATIONS.keys())}"
-            )
-
-        valid_wheel_types = VALID_CONFIGURATIONS[robot_model]["wheel_type"]
-        if wheel_type not in valid_wheel_types:
-            raise ValueError(
-                f"Invalid wheel type '{wheel_type}' for {robot_model}. "
-                f"Valid wheel types are: {', '.join(valid_wheel_types)}"
-            )
-
-    def normalize_robot_configuration(self, yaml_data: dict) -> dict:
-        """Normalizes the YAML dictionary structure to a flat to nested format and validates the structure."""
-
-        obligatory_keys = {"configuration"}
-
-        # Checking and normalize flat structure
-        if obligatory_keys.issubset(yaml_data.keys()):
-            self.validate_robot_configuration(yaml_data)
-            return {"": yaml_data}
-
-        # Checking nested structure
-        if all(isinstance(value, dict) for value in yaml_data.values()):
-            nested_data = {}
-            for namespace, robot_config in yaml_data.items():
-                if not obligatory_keys.issubset(robot_config.keys()):
-                    raise ValueError(
-                        f"Invalid nested YAML structure at '{namespace}': Missing required keys."
-                    )
-                self.validate_robot_configuration(robot_config)
-                nested_data[namespace] = robot_config
-            return nested_data
-
-        raise ValueError("Invalid YAML structure: The data does not match expected formats.")
-
-    def extract_first_robot_configuration(self, yaml_data: dict) -> Tuple[str, dict]:
-        """Extracts the namespace and configuration of first element in dict."""
-        keys = list(yaml_data.keys())
-        namespace = keys[0]
-        configuration = yaml_data[namespace]
-        return namespace, configuration
-
-    def create_launch_arguments(
-        self, namespace: str, robot_config: dict
-    ) -> list[DeclareLaunchArgument]:
-        """Declare launch arguments based on the YAML configuration files."""
-        robot_model = robot_config.get("robot_model", "panther")
-        x, y, z = robot_config.get("init_pose", [0.0, 0.0, 0.0])
-        roll, pitch, yaw = robot_config.get("init_rotation", [0.0, 0.0, 0.0])
-        x, y, z, roll, pitch, yaw = map(str, [x, y, z, roll, pitch, yaw])
-        configuration_data = robot_config.get("configuration", {})
-        wheel_type = configuration_data.get("wheel_type", "")
-
-        # Prefer declaration over configuration:
-        namespace = LaunchConfiguration("namespace", default=namespace)
-        robot_model = LaunchConfiguration("robot_model", default=robot_model)
-        wheel_type = LaunchConfiguration("wheel_type", default=wheel_type)
-        x = LaunchConfiguration("x", default=x)
-        y = LaunchConfiguration("y", default=y)
-        z = LaunchConfiguration("z", default=z)
-        roll = LaunchConfiguration("roll", default=roll)
-        pitch = LaunchConfiguration("pitch", default=pitch)
-        yaw = LaunchConfiguration("yaw", default=yaw)
-
-        return [
-            DeclareLaunchArgument(
-                "namespace",
-                default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=namespace),
-                description="Add namespace to all launched nodes.",
-            ),
-            DeclareLaunchArgument(
-                "robot_model",
-                default_value=EnvironmentVariable("ROBOT_MODEL", default_value=robot_model),
-                description="Specify robot model type.",
-            ),
-            DeclareLaunchArgument(
-                "wheel_type",
-                default_value=wheel_type,
-                description=(
-                    "Specify the wheel type. If the selected wheel type is not 'custom', "
-                    "the 'wheel_config_path' and 'controller_config_path' arguments will be "
-                    "automatically adjusted and can be omitted."
-                ),
-            ),
-            DeclareLaunchArgument(
-                "x", default_value=x, description="Initial robot position in the global 'x' axis."
-            ),
-            DeclareLaunchArgument(
-                "y", default_value=y, description="Initial robot position in the global 'y' axis."
-            ),
-            DeclareLaunchArgument(
-                "z", default_value=z, description="Initial robot position in the global 'z' axis."
-            ),
-            DeclareLaunchArgument(
-                "roll", default_value=roll, description="Initial robot 'roll' orientation."
-            ),
-            DeclareLaunchArgument(
-                "pitch", default_value=pitch, description="Initial robot 'pitch' orientation."
-            ),
-            DeclareLaunchArgument(
-                "yaw", default_value=yaw, description="Initial robot 'yaw' orientation."
-            ),
-        ]

From 6487785b878a57c8297bed30bf84f99caf862091 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Wed, 18 Sep 2024 15:37:52 +0200
Subject: [PATCH 051/100] Clean up changes

---
 lynx_description/launch/load_urdf.launch.py   |  2 +-
 .../launch/load_urdf.launch.py                |  2 +-
 .../launch/simulate_robot.launch.py           | 35 ++++++++++---------
 panther_gazebo/launch/spawn_robot.launch.py   |  5 +--
 4 files changed, 24 insertions(+), 20 deletions(-)

diff --git a/lynx_description/launch/load_urdf.launch.py b/lynx_description/launch/load_urdf.launch.py
index d4b218b4..38fb94a7 100644
--- a/lynx_description/launch/load_urdf.launch.py
+++ b/lynx_description/launch/load_urdf.launch.py
@@ -66,7 +66,7 @@ def generate_launch_description():
         ),
     )
 
-    wheel_type = LaunchConfiguration("wheel_type")  # wheel_type is used by controller_config_path
+    wheel_type = LaunchConfiguration("wheel_type")
     controller_config_path = LaunchConfiguration("controller_config_path")
     declare_controller_config_path_arg = DeclareLaunchArgument(
         "controller_config_path",
diff --git a/panther_description/launch/load_urdf.launch.py b/panther_description/launch/load_urdf.launch.py
index 930c4a83..8314bdb7 100644
--- a/panther_description/launch/load_urdf.launch.py
+++ b/panther_description/launch/load_urdf.launch.py
@@ -65,7 +65,7 @@ def generate_launch_description():
         ),
     )
 
-    wheel_type = LaunchConfiguration("wheel_type")  # wheel_type is used by controller_config_path
+    wheel_type = LaunchConfiguration("wheel_type")
     controller_config_path = LaunchConfiguration("controller_config_path")
     declare_controller_config_path_arg = DeclareLaunchArgument(
         "controller_config_path",
diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py
index 6720b18e..860d5d67 100644
--- a/panther_gazebo/launch/simulate_robot.launch.py
+++ b/panther_gazebo/launch/simulate_robot.launch.py
@@ -18,9 +18,10 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.conditions import IfCondition, UnlessCondition
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
+    EnvironmentVariable,
     LaunchConfiguration,
     PathJoinSubstitution,
     PythonExpression,
@@ -32,8 +33,6 @@
 
 def generate_launch_description():
 
-    namespace = LaunchConfiguration("namespace")
-
     add_world_transform = LaunchConfiguration("add_world_transform")
     declare_add_world_transform_arg = DeclareLaunchArgument(
         "add_world_transform",
@@ -56,10 +55,13 @@ def generate_launch_description():
         ),
     )
 
+    components_config_path = LaunchConfiguration("components_config_path")
+    robot_model = LaunchConfiguration("robot_model")
+    robot_description_pkg = PythonExpression(["'", robot_model, "_description'"])
     declare_components_config_path_arg = DeclareLaunchArgument(
         "components_config_path",
         default_value=PathJoinSubstitution(
-            [FindPackageShare("panther_description"), "config", "components.yaml"]
+            [FindPackageShare(robot_description_pkg), "config", "components.yaml"]
         ),
         description=(
             "Additional components configuration file. Components described in this file "
@@ -78,7 +80,13 @@ def generate_launch_description():
         description="Path to the parameter_bridge configuration file.",
     )
 
-    robot_model = LaunchConfiguration("robot_model")
+    namespace = LaunchConfiguration("namespace")
+    declare_namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Add namespace to all launched nodes.",
+    )
+
     robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
     robot_model_env = "lynx" if robot_model_env == "LNX" else "panther"
     declare_robot_model_arg = DeclareLaunchArgument(
@@ -115,6 +123,7 @@ def generate_launch_description():
         ),
         launch_arguments={
             "add_wheel_joints": "False",
+            "namespace": namespace,
         }.items(),
     )
 
@@ -136,16 +145,6 @@ def generate_launch_description():
         launch_arguments={"namespace": namespace, "use_sim": "True"}.items(),
     )
 
-    gz_led_strip_config = PathJoinSubstitution(
-        [FindPackageShare("panther_gazebo"), "config", "led_strips.yaml"]
-    )
-
-    gz_led_strip_config = ReplaceString(
-        source_file=gz_led_strip_config,
-        replacements={"parent_link: panther": ["parent_link: ", namespace]},
-        condition=UnlessCondition(PythonExpression(["'", namespace, "' == ''"])),
-    )
-
     controller_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -157,6 +156,7 @@ def generate_launch_description():
             )
         ),
         launch_arguments={
+            "namespace": namespace,
             "publish_robot_state": "False",
             "use_sim": "True",
         }.items(),
@@ -187,6 +187,8 @@ def generate_launch_description():
             )
         ),
         launch_arguments={
+            "components_config_path": components_config_path,
+            "namespace": namespace,
             "use_sim": "True",
         }.items(),
     )
@@ -239,9 +241,10 @@ def generate_launch_description():
     actions = [
         declare_add_world_transform_arg,
         declare_battery_config_path_arg,
+        declare_robot_model_arg,  # robot_model is used by components_config_path
         declare_components_config_path_arg,
         declare_gz_bridge_config_path_arg,
-        declare_robot_model_arg,
+        declare_namespace_arg,
         declare_use_ekf_arg,
         declare_wheel_type_arg,
         SetUseSimTime(True),
diff --git a/panther_gazebo/launch/spawn_robot.launch.py b/panther_gazebo/launch/spawn_robot.launch.py
index 4f3694ac..5637ecd6 100644
--- a/panther_gazebo/launch/spawn_robot.launch.py
+++ b/panther_gazebo/launch/spawn_robot.launch.py
@@ -85,15 +85,16 @@ def generate_launch_description():
     }
     welcome_info = welcome_msg(robot_model, "----", "simulation", log_stats)
 
-    urdf_packages = PythonExpression(["'", robot_model, "_description'"])
+    robot_description_pkg = PythonExpression(["'", robot_model, "_description'"])
     load_urdf = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare(urdf_packages), "launch", "load_urdf.launch.py"]
+                [FindPackageShare(robot_description_pkg), "launch", "load_urdf.launch.py"]
             )
         ),
         launch_arguments={
             "add_wheel_joints": LaunchConfiguration("add_wheel_joints", default="True"),
+            "namespace": namespace,
             "use_sim": "True",
         }.items(),
     )

From c1321bf48170003db870b367d1e52cf3950d659a Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Wed, 18 Sep 2024 16:48:12 +0200
Subject: [PATCH 052/100] docs update

---
 README.md | 1 -
 1 file changed, 1 deletion(-)

diff --git a/README.md b/README.md
index 1382302f..4739e131 100644
--- a/README.md
+++ b/README.md
@@ -101,7 +101,6 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | 🤖🖥️  | `localization_mode`          | Specifies the localization mode:  <br/>- 'relative' `odometry/filtered` data is relative to the initial position and orientation. <br/>- 'enu' `odometry/filtered` data is relative to initial position and ENU (East North Up) orientation. <br/> ***string:*** `relative` (choices: `relative`, `enu`)                                                    |
 | 🤖🖥️  | `namespace`                  | Add namespace to all launched nodes. <br/> ***string:*** `env(ROBOT_NAMESPACE)`                                                                                                                                                                                                                                                                               |
 | 🤖🖥️  | `publish_robot_state`        | Whether to publish the default Panther robot description. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                |
-| 🖥️   | `robot_configuration`         | Path to robot configuration YAML file. <br/> ***string:*** [`configuration.yaml`](panther_gazebo/config/configuration.yaml)                                                                                              |
 | 🖥️   | `robot_model`                 | Specify robot model type. <br/> ***string:*** `env(ROBOT_MODEL)` if not specified `panther` (choices: `lynx`, `panther`)                                                                                                                                                                                                                  |
 | 🤖🖥️  | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`PantherSafetyBT.btproj`](./panther_manager/behavior_trees/PantherSafetyBT.btproj)                                                                                                                                                                   |
 | 🤖🖥️  | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts_config.yaml`](./panther_manager/config/shutdown_hosts_config.yaml)                                                                                                                                                                                                               |

From 441a9fecb6d3bcee8bef27819317162935de3e53 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Wed, 18 Sep 2024 18:25:24 +0200
Subject: [PATCH 053/100] Z arg set to 0.0

---
 README.md                                   | 2 +-
 panther_gazebo/launch/spawn_robot.launch.py | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/README.md b/README.md
index 4739e131..4304eee1 100644
--- a/README.md
+++ b/README.md
@@ -111,7 +111,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | 🤖🖥️  | `wheel_type`                 | Specify the wheel type. If the selected wheel type is not 'custom', the wheel_config_path and controller_config_path arguments will be automatically adjusted and can be omitted. <br/> ***string:*** `WH01` (choices: `WH01`, `WH02`, `WH04`, `WH05`, `custom`) |
 | 🖥️   | `x`                          | Initial robot position in the global 'x' axis. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                          |
 | 🖥️   | `y`                          | Initial robot position in the global 'y' axis. <br/> ***float:***` -2.0`                                                                                                                                                                                                                                                                                         |
-| 🖥️   | `z`                          | Initial robot position in the global 'z' axis. <br/> ***float:*** `0.2`                                                                                                                                                                                                                                                                                          |
+| 🖥️   | `z`                          | Initial robot position in the global 'z' axis. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                          |
 | 🖥️   | `roll`                       | Initial robot 'roll' orientation. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                                       |
 | 🖥️   | `pitch`                      | Initial robot 'pitch' orientation. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                                      |
 | 🖥️   | `yaw`                        | Initial robot 'yaw' orientation. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                                        |
diff --git a/panther_gazebo/launch/spawn_robot.launch.py b/panther_gazebo/launch/spawn_robot.launch.py
index 5637ecd6..cf0d6cfa 100644
--- a/panther_gazebo/launch/spawn_robot.launch.py
+++ b/panther_gazebo/launch/spawn_robot.launch.py
@@ -61,7 +61,7 @@ def generate_launch_description():
 
     z = LaunchConfiguration("z")
     declare_z_arg = DeclareLaunchArgument(
-        "z", default_value="0.2", description="Initial robot position in the global 'z' axis."
+        "z", default_value="0.0", description="Initial robot position in the global 'z' axis."
     )
 
     roll = LaunchConfiguration("roll")

From fbc8d4c9dd95b1b3afdd2398c25a6f2381568830 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Wed, 18 Sep 2024 18:33:17 +0200
Subject: [PATCH 054/100] push unsaved

---
 panther_controller/launch/controller.launch.py | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py
index 880ceabb..10cc1de1 100644
--- a/panther_controller/launch/controller.launch.py
+++ b/panther_controller/launch/controller.launch.py
@@ -58,9 +58,7 @@ def generate_launch_description():
         ),
     )
 
-    wheel_type = LaunchConfiguration(
-        "wheel_type"
-    )  # wheel_type must be before controller_config_path
+    wheel_type = LaunchConfiguration("wheel_type")
     controller_config_path = LaunchConfiguration("controller_config_path")
     declare_controller_config_path_arg = DeclareLaunchArgument(
         "controller_config_path",

From c36470cecc51f1b5bc1bf8af2c9b8e316835aaa0 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Thu, 19 Sep 2024 10:44:49 +0200
Subject: [PATCH 055/100] Add suggestions

---
 panther_controller/launch/controller.launch.py | 14 +++++++++++++-
 panther_gazebo/launch/simulate_robot.launch.py | 17 +++--------------
 panther_gazebo/launch/spawn_robot.launch.py    |  7 ++++---
 3 files changed, 20 insertions(+), 18 deletions(-)

diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py
index 10cc1de1..c9a85602 100644
--- a/panther_controller/launch/controller.launch.py
+++ b/panther_controller/launch/controller.launch.py
@@ -94,6 +94,16 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
+    robot_model = LaunchConfiguration("robot_model")
+    robot_model_dict = {"LNX": "lynx", "PTH": "panther"}
+    robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
+    declare_robot_model_arg = DeclareLaunchArgument(
+        "robot_model",
+        default_value=robot_model_dict[robot_model_env],
+        description="Specify robot model",
+        choices=["lynx", "panther"],
+    )
+
     use_sim = LaunchConfiguration("use_sim")
     declare_use_sim_arg = DeclareLaunchArgument(
         "use_sim",
@@ -119,9 +129,10 @@ def generate_launch_description():
         ),
     )
 
+    default_wheel_type = {"lynx": "WH05", "panther": "WH01"}
     declare_wheel_type_arg = DeclareLaunchArgument(
         "wheel_type",
-        default_value="WH01",
+        default_value=PythonExpression([f"{default_wheel_type}['", robot_model, "']"]),
         description=(
             "Specify the wheel type. If the selected wheel type is not 'custom', "
             "the 'wheel_config_path' and 'controller_config_path' arguments will be "
@@ -268,6 +279,7 @@ def generate_launch_description():
 
     actions = [
         declare_battery_config_path_arg,
+        declare_robot_model_arg,  # robot_model must be before wheel_type
         declare_wheel_type_arg,  # wheel_type must be before controller_config_path
         declare_components_config_path_arg,
         declare_controller_config_path_arg,
diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py
index 860d5d67..e4dca772 100644
--- a/panther_gazebo/launch/simulate_robot.launch.py
+++ b/panther_gazebo/launch/simulate_robot.launch.py
@@ -87,11 +87,12 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
+    robot_model_dict = {"LNX": "lynx", "PTH": "panther"}
     robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
-    robot_model_env = "lynx" if robot_model_env == "LNX" else "panther"
+    robot_model_default = robot_model_dict[robot_model_env]
     declare_robot_model_arg = DeclareLaunchArgument(
         "robot_model",
-        default_value=robot_model_env,
+        default_value=robot_model_default,
         description="Specify robot model",
         choices=["lynx", "panther"],
     )
@@ -104,17 +105,6 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
-    declare_wheel_type_arg = DeclareLaunchArgument(
-        "wheel_type",
-        default_value=PythonExpression(["'WH01' if '", robot_model, "' == 'panther' else 'WH05'"]),
-        description=(
-            "Specify the wheel type. If the selected wheel type is not 'custom', "
-            "the 'wheel_config_path' and 'controller_config_path' arguments will be "
-            "automatically adjusted and can be omitted."
-        ),
-        choices=["WH01", "WH02", "WH04", "WH05", "custom"],
-    )
-
     spawn_robot_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -246,7 +236,6 @@ def generate_launch_description():
         declare_gz_bridge_config_path_arg,
         declare_namespace_arg,
         declare_use_ekf_arg,
-        declare_wheel_type_arg,
         SetUseSimTime(True),
         spawn_robot_launch,
         lights_launch,
diff --git a/panther_gazebo/launch/spawn_robot.launch.py b/panther_gazebo/launch/spawn_robot.launch.py
index cf0d6cfa..ca1b1711 100644
--- a/panther_gazebo/launch/spawn_robot.launch.py
+++ b/panther_gazebo/launch/spawn_robot.launch.py
@@ -40,11 +40,12 @@ def generate_launch_description():
     )
 
     robot_model = LaunchConfiguration("robot_model")
-    robot_model_env = os.environ.get("ROBOT_MODEL_env", default="PTH")
-    robot_model_env = "lynx" if robot_model_env == "LNX" else "panther"
+    robot_model_dict = {"LNX": "lynx", "PTH": "panther"}
+    robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
+    robot_model_default = robot_model_dict[robot_model_env]
     declare_robot_model_arg = DeclareLaunchArgument(
         "robot_model",
-        default_value=robot_model_env,
+        default_value=robot_model_default,
         description="Specify robot model",
         choices=["lynx", "panther"],
     )

From 7ac55f3bd566aff5b5a6820edb7892665aa03c77 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Thu, 19 Sep 2024 11:29:19 +0200
Subject: [PATCH 056/100] Readme

---
 README.md                                      | 2 +-
 panther_gazebo/launch/simulate_robot.launch.py | 3 +--
 panther_gazebo/launch/spawn_robot.launch.py    | 3 +--
 3 files changed, 3 insertions(+), 5 deletions(-)

diff --git a/README.md b/README.md
index 4304eee1..da2a5aff 100644
--- a/README.md
+++ b/README.md
@@ -108,7 +108,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | 🤖🖥️  | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                                             |
 | 🤖🖥️  | `user_led_animations_file`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                                                             |
 | 🤖🖥️  | `wheel_config_path`          | Path to wheel configuration file. <br/> ***string:*** [`{wheel_type}.yaml`](./panther_description/config)                                                                                                                                                                                                                                                   |
-| 🤖🖥️  | `wheel_type`                 | Specify the wheel type. If the selected wheel type is not 'custom', the wheel_config_path and controller_config_path arguments will be automatically adjusted and can be omitted. <br/> ***string:*** `WH01` (choices: `WH01`, `WH02`, `WH04`, `WH05`, `custom`) |
+| 🤖🖥️  | `wheel_type`                 | Specify the wheel type. If the selected wheel type is not 'custom', the wheel_config_path and controller_config_path arguments will be automatically adjusted and can be omitted. <br/> ***string:*** `WH01` (for Panther), `WH05` (for Lynx) (choices: `WH01`, `WH02`, `WH04`, `WH05`, `custom`) |
 | 🖥️   | `x`                          | Initial robot position in the global 'x' axis. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                          |
 | 🖥️   | `y`                          | Initial robot position in the global 'y' axis. <br/> ***float:***` -2.0`                                                                                                                                                                                                                                                                                         |
 | 🖥️   | `z`                          | Initial robot position in the global 'z' axis. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                          |
diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py
index e4dca772..cf282948 100644
--- a/panther_gazebo/launch/simulate_robot.launch.py
+++ b/panther_gazebo/launch/simulate_robot.launch.py
@@ -89,10 +89,9 @@ def generate_launch_description():
 
     robot_model_dict = {"LNX": "lynx", "PTH": "panther"}
     robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
-    robot_model_default = robot_model_dict[robot_model_env]
     declare_robot_model_arg = DeclareLaunchArgument(
         "robot_model",
-        default_value=robot_model_default,
+        default_value=robot_model_dict[robot_model_env],
         description="Specify robot model",
         choices=["lynx", "panther"],
     )
diff --git a/panther_gazebo/launch/spawn_robot.launch.py b/panther_gazebo/launch/spawn_robot.launch.py
index ca1b1711..624586ba 100644
--- a/panther_gazebo/launch/spawn_robot.launch.py
+++ b/panther_gazebo/launch/spawn_robot.launch.py
@@ -42,10 +42,9 @@ def generate_launch_description():
     robot_model = LaunchConfiguration("robot_model")
     robot_model_dict = {"LNX": "lynx", "PTH": "panther"}
     robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
-    robot_model_default = robot_model_dict[robot_model_env]
     declare_robot_model_arg = DeclareLaunchArgument(
         "robot_model",
-        default_value=robot_model_default,
+        default_value=robot_model_dict[robot_model_env],
         description="Specify robot model",
         choices=["lynx", "panther"],
     )

From 6aaa52f2bad3256c4642fce2c92d9be00d663e5e Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Thu, 19 Sep 2024 14:13:32 +0200
Subject: [PATCH 057/100] Delete panther_version argument and fix controller
 launch to use robot_model arg

---
 lynx_description/launch/load_urdf.launch.py   | 17 +++--
 .../panther_battery/battery_driver_node.hpp   |  1 -
 panther_battery/launch/battery.launch.py      |  3 -
 panther_battery/src/battery_driver_node.cpp   | 46 ++-----------
 .../test/utils/test_battery_driver_node.hpp   | 69 +++++++++----------
 .../launch/controller.launch.py               | 18 +++--
 .../launch/load_urdf.launch.py                | 13 ++--
 panther_description/urdf/panther.urdf.xacro   |  2 -
 .../urdf/panther_macro.urdf.xacro             |  5 +-
 .../test/utils/test_constants.hpp             |  1 -
 panther_manager/launch/manager.launch.py      |  7 +-
 11 files changed, 71 insertions(+), 111 deletions(-)

diff --git a/lynx_description/launch/load_urdf.launch.py b/lynx_description/launch/load_urdf.launch.py
index 38fb94a7..6e048f9c 100644
--- a/lynx_description/launch/load_urdf.launch.py
+++ b/lynx_description/launch/load_urdf.launch.py
@@ -127,14 +127,13 @@ def generate_launch_description():
         choices=["WH05", "custom"],
     )
 
-    imu_localization_x = os.environ.get("LYNX_IMU_LOCALIZATION_X", "0.168")
-    imu_localization_y = os.environ.get("LYNX_IMU_LOCALIZATION_Y", "0.028")
-    imu_localization_z = os.environ.get("LYNX_IMU_LOCALIZATION_Z", "0.083")
-    imu_orientation_r = os.environ.get("LYNX_IMU_ORIENTATION_R", "3.14")
-    imu_orientation_p = os.environ.get("LYNX_IMU_ORIENTATION_P", "-1.57")
-    imu_orientation_y = os.environ.get("LYNX_IMU_ORIENTATION_Y", "0.0")
-
     # Get URDF via xacro
+    imu_pos_x = os.environ.get("ROBOT_IMU_LOCALIZATION_X", "0.168")
+    imu_pos_y = os.environ.get("ROBOT_IMU_LOCALIZATION_Y", "0.028")
+    imu_pos_z = os.environ.get("ROBOT_IMU_LOCALIZATION_Z", "0.083")
+    imu_rot_r = os.environ.get("ROBOT_IMU_ORIENTATION_R", "3.14")
+    imu_rot_p = os.environ.get("ROBOT_IMU_ORIENTATION_P", "-1.57")
+    imu_rot_y = os.environ.get("ROBOT_IMU_ORIENTATION_Y", "0.0")
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
@@ -151,9 +150,9 @@ def generate_launch_description():
             " battery_config_file:=",
             battery_config_path,
             " imu_xyz:=",
-            f'"{imu_localization_x} {imu_localization_y} {imu_localization_z}"',
+            f"'{imu_pos_x} {imu_pos_y} {imu_pos_z}'",
             " imu_rpy:=",
-            f'"{imu_orientation_r} {imu_orientation_p} {imu_orientation_y}"',
+            f"'{imu_rot_r} {imu_rot_p} {imu_rot_y}'",
             " namespace:=",
             namespace,
             " components_config_path:=",
diff --git a/panther_battery/include/panther_battery/battery_driver_node.hpp b/panther_battery/include/panther_battery/battery_driver_node.hpp
index 0832bc03..a94725d0 100644
--- a/panther_battery/include/panther_battery/battery_driver_node.hpp
+++ b/panther_battery/include/panther_battery/battery_driver_node.hpp
@@ -42,7 +42,6 @@ class BatteryDriverNode : public rclcpp::Node
   void BatteryPubTimerCB();
   void Initialize();
   void InitializeWithADCBattery();
-  void InitializeWithRoboteqBattery();
 
   static constexpr int kADCCurrentOffset = 625;
 
diff --git a/panther_battery/launch/battery.launch.py b/panther_battery/launch/battery.launch.py
index 3719d587..51139e50 100644
--- a/panther_battery/launch/battery.launch.py
+++ b/panther_battery/launch/battery.launch.py
@@ -28,13 +28,10 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
-    panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0")
-
     battery_driver_node = Node(
         package="panther_battery",
         executable="battery_driver_node",
         name="battery_driver",
-        parameters=[{"panther_version": panther_version}],
         namespace=namespace,
         remappings=[("/diagnostics", "diagnostics")],
         emulate_tty=True,
diff --git a/panther_battery/src/battery_driver_node.cpp b/panther_battery/src/battery_driver_node.cpp
index e24ff658..6dc4eaa3 100644
--- a/panther_battery/src/battery_driver_node.cpp
+++ b/panther_battery/src/battery_driver_node.cpp
@@ -41,7 +41,6 @@ BatteryDriverNode::BatteryDriverNode(
 {
   RCLCPP_INFO(this->get_logger(), "Constructing node.");
 
-  this->declare_parameter<float>("panther_version", 1.2);
   this->declare_parameter<int>("ma_window_len/voltage", 10);
   this->declare_parameter<int>("ma_window_len/current", 10);
 
@@ -56,21 +55,14 @@ BatteryDriverNode::BatteryDriverNode(
 
 void BatteryDriverNode::Initialize()
 {
-  RCLCPP_INFO(this->get_logger(), "Initializing.");
-
-  const float panther_version = this->get_parameter("panther_version").as_double();
-  if (panther_version >= (1.2f - std::numeric_limits<float>::epsilon())) {
-    try {
-      InitializeWithADCBattery();
-      return;
-    } catch (const std::runtime_error & e) {
-      RCLCPP_WARN_STREAM(
-        this->get_logger(), "An exception occurred while initializing with ADC: "
-                              << e.what()
-                              << " Falling back to using Roboteq drivers to publish battery data.");
-    }
+  try {
+    InitializeWithADCBattery();
+  } catch (const std::runtime_error & e) {
+    RCLCPP_WARN_STREAM(
+      this->get_logger(), "An exception occurred while initializing with ADC: "
+                            << e.what()
+                            << " Falling back to using Roboteq drivers to publish battery data.");
   }
-  InitializeWithRoboteqBattery();
 
   RCLCPP_INFO(this->get_logger(), "Initialized successfully.");
 }
@@ -128,30 +120,6 @@ void BatteryDriverNode::InitializeWithADCBattery()
   RCLCPP_INFO(this->get_logger(), "Initialized battery driver using ADC data.");
 }
 
-void BatteryDriverNode::InitializeWithRoboteqBattery()
-{
-  RCLCPP_DEBUG(this->get_logger(), "Initializing with Roboteq data.");
-
-  this->declare_parameter<float>("roboteq/driver_state_timeout", 0.2);
-
-  const RoboteqBatteryParams battery_params = {
-    static_cast<float>(this->get_parameter("roboteq/driver_state_timeout").as_double()),
-    static_cast<std::size_t>(this->get_parameter("ma_window_len/voltage").as_int()),
-    static_cast<std::size_t>(this->get_parameter("ma_window_len/current").as_int()),
-  };
-
-  driver_state_sub_ = this->create_subscription<RobotDriverStateMsg>(
-    "hardware/robot_driver_state", 5,
-    [&](const RobotDriverStateMsg::SharedPtr msg) { driver_state_ = msg; });
-
-  battery_1_ = std::make_shared<RoboteqBattery>([&]() { return driver_state_; }, battery_params);
-
-  battery_publisher_ = std::make_shared<SingleBatteryPublisher>(
-    this->shared_from_this(), diagnostic_updater_, battery_1_);
-
-  RCLCPP_INFO(this->get_logger(), "Initialized battery driver using motor controllers data.");
-}
-
 void BatteryDriverNode::BatteryPubTimerCB()
 {
   if (!battery_publisher_) {
diff --git a/panther_battery/test/utils/test_battery_driver_node.hpp b/panther_battery/test/utils/test_battery_driver_node.hpp
index 4ada0103..7441af08 100644
--- a/panther_battery/test/utils/test_battery_driver_node.hpp
+++ b/panther_battery/test/utils/test_battery_driver_node.hpp
@@ -40,7 +40,7 @@ using IOStateMsg = panther_msgs::msg::IOState;
 class TestBatteryNode : public testing::Test
 {
 public:
-  TestBatteryNode(const float panther_version = 1.2, const bool dual_battery = false);
+  TestBatteryNode(const bool dual_battery = false);
   ~TestBatteryNode();
 
 protected:
@@ -63,43 +63,40 @@ class TestBatteryNode : public testing::Test
   rclcpp::Publisher<RobotDriverStateMsg>::SharedPtr driver_state_pub_;
 };
 
-TestBatteryNode::TestBatteryNode(const float panther_version, const bool dual_battery)
+TestBatteryNode::TestBatteryNode(const bool dual_battery)
 {
   std::vector<rclcpp::Parameter> params;
-  params.push_back(rclcpp::Parameter("panther_version", panther_version));
-
-  if (panther_version >= 1.2 - std::numeric_limits<float>::epsilon()) {
-    device0_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice0;
-    device1_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice1;
-
-    params.push_back(rclcpp::Parameter("adc/device0", device0_path_.string()));
-    params.push_back(rclcpp::Parameter("adc/device1", device1_path_.string()));
-    params.push_back(rclcpp::Parameter("adc/path", testing::TempDir()));
-
-    // Create the device0 and device1 directories if they do not exist
-    std::filesystem::create_directory(device0_path_);
-    std::filesystem::create_directory(device1_path_);
-
-    // Create only files that are required for adc_node to start
-    int dual_bat_volt = dual_battery ? 800 : 1600;
-    WriteNumberToFile<int>(dual_bat_volt, std::filesystem::path(device0_path_ / "in_voltage0_raw"));
-    WriteNumberToFile<int>(800, std::filesystem::path(device0_path_ / "in_voltage1_raw"));
-    WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage2_raw"));
-    WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage3_raw"));
-    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage0_scale"));
-    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage1_scale"));
-    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage2_scale"));
-    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage3_scale"));
-
-    WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage0_raw"));
-    WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage1_raw"));
-    WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage2_raw"));
-    WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage3_raw"));
-    WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage0_scale"));
-    WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage1_scale"));
-    WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage2_scale"));
-    WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage3_scale"));
-  }
+
+  device0_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice0;
+  device1_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice1;
+
+  params.push_back(rclcpp::Parameter("adc/device0", device0_path_.string()));
+  params.push_back(rclcpp::Parameter("adc/device1", device1_path_.string()));
+  params.push_back(rclcpp::Parameter("adc/path", testing::TempDir()));
+
+  // Create the device0 and device1 directories if they do not exist
+  std::filesystem::create_directory(device0_path_);
+  std::filesystem::create_directory(device1_path_);
+
+  // Create only files that are required for adc_node to start
+  int dual_bat_volt = dual_battery ? 800 : 1600;
+  WriteNumberToFile<int>(dual_bat_volt, std::filesystem::path(device0_path_ / "in_voltage0_raw"));
+  WriteNumberToFile<int>(800, std::filesystem::path(device0_path_ / "in_voltage1_raw"));
+  WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage2_raw"));
+  WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage3_raw"));
+  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage0_scale"));
+  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage1_scale"));
+  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage2_scale"));
+  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage3_scale"));
+
+  WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage0_raw"));
+  WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage1_raw"));
+  WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage2_raw"));
+  WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage3_raw"));
+  WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage0_scale"));
+  WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage1_scale"));
+  WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage2_scale"));
+  WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage3_scale"));
 
   rclcpp::NodeOptions options;
   options.parameter_overrides(params);
diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py
index c9a85602..0343163e 100644
--- a/panther_controller/launch/controller.launch.py
+++ b/panther_controller/launch/controller.launch.py
@@ -142,19 +142,25 @@ def generate_launch_description():
     )
 
     # Get URDF via xacro
+    robot_description_pkg = PythonExpression(["'", robot_model, "_description'"])
+    robot_description_file = PythonExpression(["'", robot_model, ".urdf.xacro'"])
+    imu_pos_x = os.environ.get("ROBOT_IMU_LOCALIZATION_X", "0.168")
+    imu_pos_y = os.environ.get("ROBOT_IMU_LOCALIZATION_Y", "0.028")
+    imu_pos_z = os.environ.get("ROBOT_IMU_LOCALIZATION_Z", "0.083")
+    imu_rot_r = os.environ.get("ROBOT_IMU_ORIENTATION_R", "3.14")
+    imu_rot_p = os.environ.get("ROBOT_IMU_ORIENTATION_P", "-1.57")
+    imu_rot_y = os.environ.get("ROBOT_IMU_ORIENTATION_Y", "0.0")
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
             PathJoinSubstitution(
                 [
-                    FindPackageShare("panther_description"),
+                    FindPackageShare(robot_description_pkg),
                     "urdf",
-                    "panther.urdf.xacro",
+                    robot_description_file,
                 ]
             ),
-            " panther_version:=",
-            os.environ.get("PANTHER_ROBOT_VERSION", "1.0"),
             " use_sim:=",
             use_sim,
             " wheel_config_file:=",
@@ -164,9 +170,9 @@ def generate_launch_description():
             " battery_config_file:=",
             battery_config_path,
             " imu_xyz:=",
-            f"\"{os.environ.get('PANTHER_IMU_LOCALIZATION_X', '0.168')} {os.environ.get('PANTHER_IMU_LOCALIZATION_Y', '0.028')} {os.environ.get('PANTHER_IMU_LOCALIZATION_Z', '0.083')}\"",
+            f"'{imu_pos_x} {imu_pos_y} {imu_pos_z}'",
             " imu_rpy:=",
-            f"\"{os.environ.get('PANTHER_IMU_ORIENTATION_R', '3.14')} {os.environ.get('PANTHER_IMU_ORIENTATION_P', '-1.57')} {os.environ.get('PANTHER_IMU_ORIENTATION_Y', '0.0')}\"",
+            f"'{imu_rot_r} {imu_rot_p} {imu_rot_y}'",
             " namespace:=",
             namespace,
             " components_config_path:=",
diff --git a/panther_description/launch/load_urdf.launch.py b/panther_description/launch/load_urdf.launch.py
index 8314bdb7..8a0f8ee6 100644
--- a/panther_description/launch/load_urdf.launch.py
+++ b/panther_description/launch/load_urdf.launch.py
@@ -126,8 +126,13 @@ def generate_launch_description():
         choices=["WH01", "WH02", "WH04", "custom"],
     )
 
-    panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0")
     # Get URDF via xacro
+    imu_pos_x = os.environ.get("ROBOT_IMU_LOCALIZATION_X", "0.168")
+    imu_pos_y = os.environ.get("ROBOT_IMU_LOCALIZATION_Y", "0.028")
+    imu_pos_z = os.environ.get("ROBOT_IMU_LOCALIZATION_Z", "0.083")
+    imu_rot_r = os.environ.get("ROBOT_IMU_ORIENTATION_R", "3.14")
+    imu_rot_p = os.environ.get("ROBOT_IMU_ORIENTATION_P", "-1.57")
+    imu_rot_y = os.environ.get("ROBOT_IMU_ORIENTATION_Y", "0.0")
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
@@ -135,8 +140,6 @@ def generate_launch_description():
             PathJoinSubstitution(
                 [FindPackageShare("panther_description"), "urdf", "panther.urdf.xacro"]
             ),
-            " panther_version:=",
-            panther_version,
             " use_sim:=",
             use_sim,
             " wheel_config_file:=",
@@ -146,9 +149,9 @@ def generate_launch_description():
             " battery_config_file:=",
             battery_config_path,
             " imu_xyz:=",
-            f"\"{os.environ.get('PANTHER_IMU_LOCALIZATION_X', '0.168')} {os.environ.get('PANTHER_IMU_LOCALIZATION_Y', '0.028')} {os.environ.get('PANTHER_IMU_LOCALIZATION_Z', '0.083')}\"",
+            f"'{imu_pos_x} {imu_pos_y} {imu_pos_z}'",
             " imu_rpy:=",
-            f"\"{os.environ.get('PANTHER_IMU_ORIENTATION_R', '3.14')} {os.environ.get('PANTHER_IMU_ORIENTATION_P', '-1.57')} {os.environ.get('PANTHER_IMU_ORIENTATION_Y', '0.0')}\"",
+            f"'{imu_rot_r} {imu_rot_p} {imu_rot_y}'",
             " namespace:=",
             namespace,
             " components_config_path:=",
diff --git a/panther_description/urdf/panther.urdf.xacro b/panther_description/urdf/panther.urdf.xacro
index 25e4c754..1c167186 100644
--- a/panther_description/urdf/panther.urdf.xacro
+++ b/panther_description/urdf/panther.urdf.xacro
@@ -1,7 +1,6 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot name="panther" xmlns:xacro="http://wiki.ros.org/xacro">
 
-  <xacro:arg name="panther_version" default="1.0" />
   <xacro:arg name="use_sim" default="false" />
   <!-- if lidars have to work correctly, use_gpu has to be true. -->
   <xacro:arg name="use_gpu" default="true" />
@@ -18,7 +17,6 @@
 
   <xacro:include filename="$(find panther_description)/urdf/panther_macro.urdf.xacro" ns="husarion" />
   <xacro:husarion.panther_robot
-    panther_version="$(arg panther_version)"
     use_sim="$(arg use_sim)"
     imu_xyz="$(arg imu_xyz)"
     imu_rpy="$(arg imu_rpy)"
diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro
index 4911f276..de7f32ef 100644
--- a/panther_description/urdf/panther_macro.urdf.xacro
+++ b/panther_description/urdf/panther_macro.urdf.xacro
@@ -3,8 +3,7 @@
 
   <xacro:macro
     name="panther_robot"
-    params="panther_version
-            wheel_config_file
+    params="wheel_config_file
             controller_config_file
             battery_config_file
             use_sim:=false
@@ -64,8 +63,6 @@
         <xacro:unless value="${use_sim}">
           <plugin>panther_hardware_interfaces/PantherSystem</plugin>
 
-          <param name="panther_version">${panther_version}</param>
-
           <param name="encoder_resolution">1600</param>
 
           <param name="gear_ratio">30.08</param>
diff --git a/panther_hardware_interfaces/test/utils/test_constants.hpp b/panther_hardware_interfaces/test/utils/test_constants.hpp
index 2c34ce42..c3d4a0fa 100644
--- a/panther_hardware_interfaces/test/utils/test_constants.hpp
+++ b/panther_hardware_interfaces/test/utils/test_constants.hpp
@@ -67,7 +67,6 @@ const std::string kPluginName =
 )";
 
 const std::map<std::string, std::string> kDefaultParamMap = {
-  {"panther_version", "1.2"},
   {"encoder_resolution", "1600"},
   {"gear_ratio", "30.08"},
   {"gearbox_efficiency", "0.75"},
diff --git a/panther_manager/launch/manager.launch.py b/panther_manager/launch/manager.launch.py
index 14f4c8c7..510fb283 100644
--- a/panther_manager/launch/manager.launch.py
+++ b/panther_manager/launch/manager.launch.py
@@ -16,12 +16,11 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition
+from launch.conditions import UnlessCondition
 from launch.substitutions import (
     EnvironmentVariable,
     LaunchConfiguration,
     PathJoinSubstitution,
-    PythonExpression,
 )
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -29,7 +28,6 @@
 
 def generate_launch_description():
 
-    panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0")
     panther_manager_dir = FindPackageShare("panther_manager")
 
     lights_bt_project_path = LaunchConfiguration("lights_bt_project_path")
@@ -55,7 +53,6 @@ def generate_launch_description():
             [panther_manager_dir, "behavior_trees", "PantherSafetyBT.btproj"]
         ),
         description="Path to BehaviorTree project file, responsible for safety and shutdown management.",
-        condition=IfCondition(PythonExpression([panther_version, ">=", "1.2"])),
     )
 
     shutdown_hosts_config_path = LaunchConfiguration("shutdown_hosts_config_path")
@@ -103,7 +100,7 @@ def generate_launch_description():
         ],
         namespace=namespace,
         emulate_tty=True,
-        condition=IfCondition(PythonExpression([panther_version, ">=", "1.2 and not ", use_sim])),
+        condition=UnlessCondition(use_sim),
     )
 
     actions = [

From 21570ee2ca64cfc02c2a90ecb34637c149aed96f Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 20 Sep 2024 12:01:21 +0200
Subject: [PATCH 058/100] panther-verion -> use_adc_battery

---
 .../panther_battery/battery_driver_node.hpp   |  1 +
 panther_battery/src/battery_driver_node.cpp   | 46 +++++++++++--
 .../test_battery_driver_node_adc_dual.cpp     |  2 +-
 .../test/utils/test_battery_driver_node.hpp   | 69 ++++++++++---------
 4 files changed, 77 insertions(+), 41 deletions(-)

diff --git a/panther_battery/include/panther_battery/battery_driver_node.hpp b/panther_battery/include/panther_battery/battery_driver_node.hpp
index a94725d0..0832bc03 100644
--- a/panther_battery/include/panther_battery/battery_driver_node.hpp
+++ b/panther_battery/include/panther_battery/battery_driver_node.hpp
@@ -42,6 +42,7 @@ class BatteryDriverNode : public rclcpp::Node
   void BatteryPubTimerCB();
   void Initialize();
   void InitializeWithADCBattery();
+  void InitializeWithRoboteqBattery();
 
   static constexpr int kADCCurrentOffset = 625;
 
diff --git a/panther_battery/src/battery_driver_node.cpp b/panther_battery/src/battery_driver_node.cpp
index 6dc4eaa3..122d1d16 100644
--- a/panther_battery/src/battery_driver_node.cpp
+++ b/panther_battery/src/battery_driver_node.cpp
@@ -41,6 +41,7 @@ BatteryDriverNode::BatteryDriverNode(
 {
   RCLCPP_INFO(this->get_logger(), "Constructing node.");
 
+  this->declare_parameter<bool>("use_adc_battery", true);
   this->declare_parameter<int>("ma_window_len/voltage", 10);
   this->declare_parameter<int>("ma_window_len/current", 10);
 
@@ -55,14 +56,21 @@ BatteryDriverNode::BatteryDriverNode(
 
 void BatteryDriverNode::Initialize()
 {
-  try {
-    InitializeWithADCBattery();
-  } catch (const std::runtime_error & e) {
-    RCLCPP_WARN_STREAM(
-      this->get_logger(), "An exception occurred while initializing with ADC: "
-                            << e.what()
-                            << " Falling back to using Roboteq drivers to publish battery data.");
+  RCLCPP_INFO(this->get_logger(), "Initializing.");
+
+  const float use_adc_battery = this->get_parameter("use_adc_battery").as_bool();
+  if (use_adc_battery) {
+    try {
+      InitializeWithADCBattery();
+      return;
+    } catch (const std::runtime_error & e) {
+      RCLCPP_WARN_STREAM(
+        this->get_logger(), "An exception occurred while initializing with ADC: "
+                              << e.what()
+                              << " Falling back to using Roboteq drivers to publish battery data.");
+    }
   }
+  InitializeWithRoboteqBattery();
 
   RCLCPP_INFO(this->get_logger(), "Initialized successfully.");
 }
@@ -120,6 +128,30 @@ void BatteryDriverNode::InitializeWithADCBattery()
   RCLCPP_INFO(this->get_logger(), "Initialized battery driver using ADC data.");
 }
 
+void BatteryDriverNode::InitializeWithRoboteqBattery()
+{
+  RCLCPP_DEBUG(this->get_logger(), "Initializing with Roboteq data.");
+
+  this->declare_parameter<float>("roboteq/driver_state_timeout", 0.2);
+
+  const RoboteqBatteryParams battery_params = {
+    static_cast<float>(this->get_parameter("roboteq/driver_state_timeout").as_double()),
+    static_cast<std::size_t>(this->get_parameter("ma_window_len/voltage").as_int()),
+    static_cast<std::size_t>(this->get_parameter("ma_window_len/current").as_int()),
+  };
+
+  driver_state_sub_ = this->create_subscription<RobotDriverStateMsg>(
+    "hardware/robot_driver_state", 5,
+    [&](const RobotDriverStateMsg::SharedPtr msg) { driver_state_ = msg; });
+
+  battery_1_ = std::make_shared<RoboteqBattery>([&]() { return driver_state_; }, battery_params);
+
+  battery_publisher_ = std::make_shared<SingleBatteryPublisher>(
+    this->shared_from_this(), diagnostic_updater_, battery_1_);
+
+  RCLCPP_INFO(this->get_logger(), "Initialized battery driver using motor controllers data.");
+}
+
 void BatteryDriverNode::BatteryPubTimerCB()
 {
   if (!battery_publisher_) {
diff --git a/panther_battery/test/test_battery_driver_node_adc_dual.cpp b/panther_battery/test/test_battery_driver_node_adc_dual.cpp
index 414fce6b..740ba838 100644
--- a/panther_battery/test/test_battery_driver_node_adc_dual.cpp
+++ b/panther_battery/test/test_battery_driver_node_adc_dual.cpp
@@ -25,7 +25,7 @@
 class TestBatteryNodeADCDual : public TestBatteryNode
 {
 public:
-  TestBatteryNodeADCDual() : TestBatteryNode(1.2, true) {}
+  TestBatteryNodeADCDual() : TestBatteryNode(true, true) {}
 };
 
 TEST_F(TestBatteryNodeADCDual, BatteryValues)
diff --git a/panther_battery/test/utils/test_battery_driver_node.hpp b/panther_battery/test/utils/test_battery_driver_node.hpp
index 7441af08..a58875b1 100644
--- a/panther_battery/test/utils/test_battery_driver_node.hpp
+++ b/panther_battery/test/utils/test_battery_driver_node.hpp
@@ -40,7 +40,7 @@ using IOStateMsg = panther_msgs::msg::IOState;
 class TestBatteryNode : public testing::Test
 {
 public:
-  TestBatteryNode(const bool dual_battery = false);
+  TestBatteryNode(const bool use_adc_battery = true, const bool dual_battery = false);
   ~TestBatteryNode();
 
 protected:
@@ -63,40 +63,43 @@ class TestBatteryNode : public testing::Test
   rclcpp::Publisher<RobotDriverStateMsg>::SharedPtr driver_state_pub_;
 };
 
-TestBatteryNode::TestBatteryNode(const bool dual_battery)
+TestBatteryNode::TestBatteryNode(const bool use_adc_battery, const bool dual_battery)
 {
   std::vector<rclcpp::Parameter> params;
-
-  device0_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice0;
-  device1_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice1;
-
-  params.push_back(rclcpp::Parameter("adc/device0", device0_path_.string()));
-  params.push_back(rclcpp::Parameter("adc/device1", device1_path_.string()));
-  params.push_back(rclcpp::Parameter("adc/path", testing::TempDir()));
-
-  // Create the device0 and device1 directories if they do not exist
-  std::filesystem::create_directory(device0_path_);
-  std::filesystem::create_directory(device1_path_);
-
-  // Create only files that are required for adc_node to start
-  int dual_bat_volt = dual_battery ? 800 : 1600;
-  WriteNumberToFile<int>(dual_bat_volt, std::filesystem::path(device0_path_ / "in_voltage0_raw"));
-  WriteNumberToFile<int>(800, std::filesystem::path(device0_path_ / "in_voltage1_raw"));
-  WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage2_raw"));
-  WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage3_raw"));
-  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage0_scale"));
-  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage1_scale"));
-  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage2_scale"));
-  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage3_scale"));
-
-  WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage0_raw"));
-  WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage1_raw"));
-  WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage2_raw"));
-  WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage3_raw"));
-  WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage0_scale"));
-  WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage1_scale"));
-  WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage2_scale"));
-  WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage3_scale"));
+  params.push_back(rclcpp::Parameter("use_adc_battery", use_adc_battery));
+
+  if (use_adc_battery) {
+    device0_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice0;
+    device1_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice1;
+
+    params.push_back(rclcpp::Parameter("adc/device0", device0_path_.string()));
+    params.push_back(rclcpp::Parameter("adc/device1", device1_path_.string()));
+    params.push_back(rclcpp::Parameter("adc/path", testing::TempDir()));
+
+    // Create the device0 and device1 directories if they do not exist
+    std::filesystem::create_directory(device0_path_);
+    std::filesystem::create_directory(device1_path_);
+
+    // Create only files that are required for adc_node to start
+    int dual_bat_volt = dual_battery ? 800 : 1600;
+    WriteNumberToFile<int>(dual_bat_volt, std::filesystem::path(device0_path_ / "in_voltage0_raw"));
+    WriteNumberToFile<int>(800, std::filesystem::path(device0_path_ / "in_voltage1_raw"));
+    WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage2_raw"));
+    WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage3_raw"));
+    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage0_scale"));
+    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage1_scale"));
+    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage2_scale"));
+    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage3_scale"));
+
+    WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage0_raw"));
+    WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage1_raw"));
+    WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage2_raw"));
+    WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage3_raw"));
+    WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage0_scale"));
+    WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage1_scale"));
+    WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage2_scale"));
+    WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage3_scale"));
+  }
 
   rclcpp::NodeOptions options;
   options.parameter_overrides(params);

From c4b2b120c26de1342a9d005051e2be9cf0523638 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 20 Sep 2024 13:22:45 +0200
Subject: [PATCH 059/100] push unsaved

---
 panther_battery/test/test_battery_driver_node_adc_single.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/panther_battery/test/test_battery_driver_node_adc_single.cpp b/panther_battery/test/test_battery_driver_node_adc_single.cpp
index 21f6995f..cc26bfe7 100644
--- a/panther_battery/test/test_battery_driver_node_adc_single.cpp
+++ b/panther_battery/test/test_battery_driver_node_adc_single.cpp
@@ -25,7 +25,7 @@
 class TestBatteryNodeADCSingle : public TestBatteryNode
 {
 public:
-  TestBatteryNodeADCSingle() : TestBatteryNode(1.2, false) {}
+  TestBatteryNodeADCSingle() : TestBatteryNode(true, false) {}
 };
 
 TEST_F(TestBatteryNodeADCSingle, BatteryValues)

From 5f6469425091a74113a8554499b089c6792cdbb4 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 20 Sep 2024 13:27:14 +0200
Subject: [PATCH 060/100] Fix test

---
 panther_battery/test/test_battery_driver_node_roboteq.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/panther_battery/test/test_battery_driver_node_roboteq.cpp b/panther_battery/test/test_battery_driver_node_roboteq.cpp
index 611b5d64..84eb00b9 100644
--- a/panther_battery/test/test_battery_driver_node_roboteq.cpp
+++ b/panther_battery/test/test_battery_driver_node_roboteq.cpp
@@ -27,7 +27,7 @@
 class TestBatteryNodeRoboteq : public TestBatteryNode
 {
 public:
-  TestBatteryNodeRoboteq() : TestBatteryNode(1.0) {}
+  TestBatteryNodeRoboteq() : TestBatteryNode(false) {}
 };
 
 TEST_F(TestBatteryNodeRoboteq, BatteryValues)

From 3aa73ef498905efcbbac35397a43d0d4def9d5db Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 20 Sep 2024 13:32:15 +0200
Subject: [PATCH 061/100] Add lynx_description to pkg xml

---
 panther_controller/package.xml | 1 +
 panther_gazebo/package.xml     | 1 +
 2 files changed, 2 insertions(+)

diff --git a/panther_controller/package.xml b/panther_controller/package.xml
index 067db90c..6dea3a07 100644
--- a/panther_controller/package.xml
+++ b/panther_controller/package.xml
@@ -22,6 +22,7 @@
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_ros</exec_depend>
+  <exec_depend>lynx_description</exec_depend>
   <exec_depend>mecanum_drive_controller</exec_depend>
   <exec_depend>panther_description</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">panther_hardware_interfaces</exec_depend>
diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml
index ef2d3d6b..faa23cdd 100644
--- a/panther_gazebo/package.xml
+++ b/panther_gazebo/package.xml
@@ -31,6 +31,7 @@
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">husarion_gz_worlds</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">launch</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">launch_ros</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">lynx_description</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">nav2_common</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_controller</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_description</exec_depend>

From 6778b072992b328961b0579ea5ee83e2703ae288 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 20 Sep 2024 13:39:20 +0200
Subject: [PATCH 062/100] Fix wheel type path

---
 panther_controller/launch/controller.launch.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py
index 0343163e..9dc30b09 100644
--- a/panther_controller/launch/controller.launch.py
+++ b/panther_controller/launch/controller.launch.py
@@ -112,12 +112,13 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
+    robot_description_pkg = PythonExpression(["'", robot_model, "_description'"])
     wheel_config_path = LaunchConfiguration("wheel_config_path")
     declare_wheel_config_path_arg = DeclareLaunchArgument(
         "wheel_config_path",
         default_value=PathJoinSubstitution(
             [
-                FindPackageShare("panther_description"),
+                FindPackageShare(robot_description_pkg),
                 "config",
                 PythonExpression(["'", wheel_type, ".yaml'"]),
             ]

From 340baccae353fc405e4d0b308c94fb33bf107202 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 20 Sep 2024 14:05:42 +0200
Subject: [PATCH 063/100] Delete param

---
 panther_battery/src/battery_driver_node.cpp | 20 ++++++++------------
 1 file changed, 8 insertions(+), 12 deletions(-)

diff --git a/panther_battery/src/battery_driver_node.cpp b/panther_battery/src/battery_driver_node.cpp
index 122d1d16..f3603b87 100644
--- a/panther_battery/src/battery_driver_node.cpp
+++ b/panther_battery/src/battery_driver_node.cpp
@@ -41,7 +41,6 @@ BatteryDriverNode::BatteryDriverNode(
 {
   RCLCPP_INFO(this->get_logger(), "Constructing node.");
 
-  this->declare_parameter<bool>("use_adc_battery", true);
   this->declare_parameter<int>("ma_window_len/voltage", 10);
   this->declare_parameter<int>("ma_window_len/current", 10);
 
@@ -58,17 +57,14 @@ void BatteryDriverNode::Initialize()
 {
   RCLCPP_INFO(this->get_logger(), "Initializing.");
 
-  const float use_adc_battery = this->get_parameter("use_adc_battery").as_bool();
-  if (use_adc_battery) {
-    try {
-      InitializeWithADCBattery();
-      return;
-    } catch (const std::runtime_error & e) {
-      RCLCPP_WARN_STREAM(
-        this->get_logger(), "An exception occurred while initializing with ADC: "
-                              << e.what()
-                              << " Falling back to using Roboteq drivers to publish battery data.");
-    }
+  try {
+    InitializeWithADCBattery();
+    return;
+  } catch (const std::runtime_error & e) {
+    RCLCPP_WARN_STREAM(
+      this->get_logger(), "An exception occurred while initializing with ADC: "
+                            << e.what()
+                            << " Falling back to using Roboteq drivers to publish battery data.");
   }
   InitializeWithRoboteqBattery();
 

From 9c0a8c1ec9f9dde91a8b9fb3287a8fd0f3ab60e6 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 20 Sep 2024 14:12:14 +0200
Subject: [PATCH 064/100] Dawid suggestion

---
 panther_battery/test/utils/test_battery_driver_node.hpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/panther_battery/test/utils/test_battery_driver_node.hpp b/panther_battery/test/utils/test_battery_driver_node.hpp
index a58875b1..bad9542c 100644
--- a/panther_battery/test/utils/test_battery_driver_node.hpp
+++ b/panther_battery/test/utils/test_battery_driver_node.hpp
@@ -66,7 +66,6 @@ class TestBatteryNode : public testing::Test
 TestBatteryNode::TestBatteryNode(const bool use_adc_battery, const bool dual_battery)
 {
   std::vector<rclcpp::Parameter> params;
-  params.push_back(rclcpp::Parameter("use_adc_battery", use_adc_battery));
 
   if (use_adc_battery) {
     device0_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice0;

From 62e843674e5359470914e0d0a4ec494079df928e Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Mon, 30 Sep 2024 16:20:38 +0200
Subject: [PATCH 065/100] Unify arguments and config names (#416)

* Unify arguments and config names

* led_config_file -> animations_config_path
---
 .gitignore                                    |  6 ++----
 README.md                                     |  7 +++++--
 panther_gazebo/CONFIGURATION.md               |  2 +-
 panther_gazebo/README.md                      |  2 +-
 ...plugin_config.yaml => battery_plugin.yaml} |  0
 .../launch/simulate_robot.launch.py           |  2 +-
 panther_lights/README.md                      |  4 ++--
 .../panther_lights/lights_controller_node.hpp |  4 ++--
 panther_lights/launch/lights.launch.py        | 20 +++++++++----------
 panther_lights/src/lights_controller_node.cpp | 18 ++++++++---------
 .../test/integration/panther_lights.test.py   |  4 ++--
 .../test/unit/test_lights_controller_node.cpp | 13 +++++++-----
 panther_manager/CONFIGURATION.md              |  2 +-
 panther_manager/README.md                     |  2 +-
 ..._hosts_config.yaml => shutdown_hosts.yaml} |  0
 panther_manager/launch/manager.launch.py      |  2 +-
 16 files changed, 46 insertions(+), 42 deletions(-)
 rename panther_gazebo/config/{battery_plugin_config.yaml => battery_plugin.yaml} (100%)
 rename panther_manager/config/{shutdown_hosts_config.yaml => shutdown_hosts.yaml} (100%)

diff --git a/.gitignore b/.gitignore
index f42d579a..ac9f2a85 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,6 +1,6 @@
-devel/
-logs/
 build/
+log/
+install/
 bin/
 lib/
 msg_gen/
@@ -13,8 +13,6 @@ msg/*Feedback.msg
 msg/*Goal.msg
 msg/*Result.msg
 msg/_*.py
-build_isolated/
-devel_isolated/
 
 # Generated by dynamic reconfigure
 *.cfgc
diff --git a/README.md b/README.md
index da2a5aff..e887b3cb 100644
--- a/README.md
+++ b/README.md
@@ -71,6 +71,9 @@ Simulation:
 ros2 launch panther_gazebo simulation.launch.py
 ```
 
+> [!IMPORTANT]
+> You can change spawning robot in simulation, by adding `robot_model:={robot_model}` argument.
+
 ### Launch Arguments
 
 Launch arguments are largely common to both simulation and physical robot. However, there is a group of arguments that apply only to hardware or only to the simulator. Below is a legend to the tables with all launch arguments.
@@ -84,6 +87,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | --- | ---------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
 | 🖥️   | `add_wheel_joints`           | Flag enabling joint_state_publisher to publish information about the wheel position. Should be false when there is a controller that sends this information. <br/> ***bool:*** `False`                                                                                                                                              |
 | 🖥️   | `add_world_transform`        | Adds a world frame that connects the tf trees of individual robots (useful when running multiple robots). <br/> ***bool:*** `False`                                                                                                                                                                                                                                              |
+| 🤖🖥️  | `animations_config_path`    | Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. <br/> ***string:*** [`led_config.yaml`](./panther_lights/config/led_config.yaml)                                                                                                                        |
 | 🖥️   | `battery_config_path`        | Path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only. <br/> ***string:*** `None`                                                                                                                                                                                                           |
 | 🤖🖥️  | `components_config_path`     | Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options). <br/> ***string:*** [`components.yaml`](./panther_description/config/components.yaml)                                          |
 | 🤖🖥️  | `controller_config_path`     | Path to controller configuration file. A path to custom configuration can be specified here. <br/> ***string:*** [`{wheel_type}_controller.yaml`](./panther_controller/config/)                                                                                                                                                                              |
@@ -95,7 +99,6 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | 🖥️   | `gz_log_level`               | Adjust the level of console output. <br/> ***int:*** `1` (choices: `0`, `1`, `2`, `3`, `4`)                                                                                                                                                                                                                                                                               |
 | 🖥️   | `gz_world`                   | Absolute path to SDF world file. <br/> ***string:*** [`husarion_world.sdf`](https://github.com/husarion/husarion_gz_worlds/blob/main/worlds/husarion_world.sdf)                                                                                                                                                                                                 |
 | 🤖   | `launch_nmea_gps`            | Whether to launch the NMEA NavSat driver node. Advisable when the robot is equipped with the [ANT02](https://husarion.com/manuals/panther/panther-options/#ant02---wi-fi--lte--gps). <br/> ***bool:*** `False`   |
-| 🤖🖥️  | `led_config_file`            | Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. <br/> ***string:*** [`led_config.yaml`](./panther_lights/config/led_config.yaml)                                                                                                                        |
 | 🤖🖥️  | `lights_bt_project_path`     | Path to BehaviorTree project file, responsible for lights management. <br/> ***string:*** [`PantherLightsBT.btproj`](./panther_manager/behavior_trees/PantherLightsBT.btproj)                                                                                                                                                                                |
 | 🤖🖥️  | `localization_config_path`   | Specify the path to the localization configuration file. <br/> ***string:*** [`relative_localization.yaml`](./panther_localization/config/relative_localization.yaml)                                                                                                                                                                                          |
 | 🤖🖥️  | `localization_mode`          | Specifies the localization mode:  <br/>- 'relative' `odometry/filtered` data is relative to the initial position and orientation. <br/>- 'enu' `odometry/filtered` data is relative to initial position and ENU (East North Up) orientation. <br/> ***string:*** `relative` (choices: `relative`, `enu`)                                                    |
@@ -106,7 +109,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | 🤖🖥️  | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts_config.yaml`](./panther_manager/config/shutdown_hosts_config.yaml)                                                                                                                                                                                                               |
 | 🤖🖥️  | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                                   |
 | 🤖🖥️  | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                                             |
-| 🤖🖥️  | `user_led_animations_file`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                                                             |
+| 🤖🖥️  | `user_led_animations_path`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                                                             |
 | 🤖🖥️  | `wheel_config_path`          | Path to wheel configuration file. <br/> ***string:*** [`{wheel_type}.yaml`](./panther_description/config)                                                                                                                                                                                                                                                   |
 | 🤖🖥️  | `wheel_type`                 | Specify the wheel type. If the selected wheel type is not 'custom', the wheel_config_path and controller_config_path arguments will be automatically adjusted and can be omitted. <br/> ***string:*** `WH01` (for Panther), `WH05` (for Lynx) (choices: `WH01`, `WH02`, `WH04`, `WH05`, `custom`) |
 | 🖥️   | `x`                          | Initial robot position in the global 'x' axis. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                          |
diff --git a/panther_gazebo/CONFIGURATION.md b/panther_gazebo/CONFIGURATION.md
index c41cc924..d83baf52 100644
--- a/panther_gazebo/CONFIGURATION.md
+++ b/panther_gazebo/CONFIGURATION.md
@@ -33,7 +33,7 @@ To obtain GPS data in Ignition, follow these steps:
 
 ## Linear Battery Plugin
 
-It is possible to simulate the battery operation of the Panther robot. By default, this feature is disabled, but you can enable it by setting the `simulate_discharging` parameter to `true` in the `battery_plugin_config.yaml` file or in the file pointed to by the `battery_config_path` parameter. Below, you will find the plugin parameters that enable battery simulation.
+It is possible to simulate the battery operation of the Panther robot. By default, this feature is disabled, but you can enable it by setting the `simulate_discharging` parameter to `true` in the `battery_plugin.yaml` file or in the file pointed to by the `battery_config_path` parameter. Below, you will find the plugin parameters that enable battery simulation.
 
 - `simulate_discharging` [*bool*, default: **false**]: Enables battery simulation. If set to `true`, the battery will discharge **at a constant rate** (regardless of joint torque), and if it depletes completely, the robot will stop moving. When set to `false`, the battery will not discharge, but the battery status information will still be published on the `battery/battery_status` topic.
 - `initial_charge_percentage` [*float*, default: **70.0**]: Sets the initial charge percentage of the battery.
diff --git a/panther_gazebo/README.md b/panther_gazebo/README.md
index a4d3bb85..7a818915 100644
--- a/panther_gazebo/README.md
+++ b/panther_gazebo/README.md
@@ -11,7 +11,7 @@ The package contains a launch file and source files used to run the robot simula
 
 ## Configuration Files
 
-- [`battery_plugin_config.yaml`](./config/battery_plugin_config.yaml): Simulated LinearBatteryPlugin configuration.
+- [`battery_plugin.yaml`](./config/battery_plugin.yaml): Simulated LinearBatteryPlugin configuration.
 - [`gz_bridge.yaml`](./config/gz_bridge.yaml): Specify data to exchange between ROS and Gazebo simulation.
 - [`teleop_with_estop.config`](./config/teleop_with_estop.config): Gazebo layout configuration file, which adds E-Stop and Teleop widgets.
 
diff --git a/panther_gazebo/config/battery_plugin_config.yaml b/panther_gazebo/config/battery_plugin.yaml
similarity index 100%
rename from panther_gazebo/config/battery_plugin_config.yaml
rename to panther_gazebo/config/battery_plugin.yaml
diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py
index cf282948..2bd4f822 100644
--- a/panther_gazebo/launch/simulate_robot.launch.py
+++ b/panther_gazebo/launch/simulate_robot.launch.py
@@ -51,7 +51,7 @@ def generate_launch_description():
             "This configuration is intended for use in simulations only."
         ),
         default_value=PathJoinSubstitution(
-            [FindPackageShare("panther_gazebo"), "config", "battery_plugin_config.yaml"]
+            [FindPackageShare("panther_gazebo"), "config", "battery_plugin.yaml"]
         ),
     )
 
diff --git a/panther_lights/README.md b/panther_lights/README.md
index 8c1a7331..44b288c1 100644
--- a/panther_lights/README.md
+++ b/panther_lights/README.md
@@ -29,9 +29,9 @@ This node is of type rclcpp_components is responsible for processing animations
 
 #### Parameters
 
+- `~animations_config_path` [*string*, default: **$(find panther_lights)/panther_lights/config/led_config.yaml**]: Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations.
 - `~controller_frequency` [*float*, default: **50.0**]: Frequency [Hz] at which the lights controller node will process animations.
-- `~led_config_file` [*string*, default: **$(find panther_lights)/panther_lights/config/led_config.yaml**]: Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations.
-- `~user_led_animations_file` [*string*, default: **None**]: Path to a YAML file with a description of the user defined animations.
+- `~user_led_animations_path` [*string*, default: **None**]: Path to a YAML file with a description of the user defined animations.
 
 ### LightsDriverNode
 
diff --git a/panther_lights/include/panther_lights/lights_controller_node.hpp b/panther_lights/include/panther_lights/lights_controller_node.hpp
index ef051bd3..eb98f16f 100644
--- a/panther_lights/include/panther_lights/lights_controller_node.hpp
+++ b/panther_lights/include/panther_lights/lights_controller_node.hpp
@@ -90,9 +90,9 @@ class LightsControllerNode : public rclcpp::Node
   /**
    * @brief Adds animations to an unordered map with animations
    *
-   * @param user_led_animations_file path to YAML file with user animations description
+   * @param user_led_animations_path path to YAML file with user animations description
    */
-  void LoadUserAnimations(const std::string & user_led_animations_file);
+  void LoadUserAnimations(const std::string & user_led_animations_path);
 
   /**
    * @brief Adds animation to an unordered map with animations
diff --git a/panther_lights/launch/lights.launch.py b/panther_lights/launch/lights.launch.py
index f42846f5..eced7392 100644
--- a/panther_lights/launch/lights.launch.py
+++ b/panther_lights/launch/lights.launch.py
@@ -29,9 +29,9 @@
 
 
 def generate_launch_description():
-    led_config_file = LaunchConfiguration("led_config_file")
-    declare_led_config_file_arg = DeclareLaunchArgument(
-        "led_config_file",
+    animations_config_path = LaunchConfiguration("animations_config_path")
+    declare_animations_config_path_arg = DeclareLaunchArgument(
+        "animations_config_path",
         default_value=PathJoinSubstitution(
             [FindPackageShare("panther_lights"), "config", "led_config.yaml"]
         ),
@@ -52,9 +52,9 @@ def generate_launch_description():
         description="Whether simulation is used",
     )
 
-    user_led_animations_file = LaunchConfiguration("user_led_animations_file")
-    declare_user_led_animations_file_arg = DeclareLaunchArgument(
-        "user_led_animations_file",
+    user_led_animations_path = LaunchConfiguration("user_led_animations_path")
+    declare_user_led_animations_path_arg = DeclareLaunchArgument(
+        "user_led_animations_path",
         default_value="",
         description="Path to a YAML file with a description of the user defined animations.",
     )
@@ -82,8 +82,8 @@ def generate_launch_description():
                 name="lights_controller",
                 namespace=namespace,
                 parameters=[
-                    {"led_config_file": led_config_file},
-                    {"user_led_animations_file": user_led_animations_file},
+                    {"animations_config_path": animations_config_path},
+                    {"user_led_animations_path": user_led_animations_path},
                 ],
                 extra_arguments=[
                     {"use_intra_process_comms": True},
@@ -95,10 +95,10 @@ def generate_launch_description():
     )
 
     actions = [
-        declare_led_config_file_arg,
+        declare_animations_config_path_arg,
         declare_namespace_arg,
         declare_use_sim_arg,
-        declare_user_led_animations_file_arg,
+        declare_user_led_animations_path_arg,
         lights_container,
     ]
 
diff --git a/panther_lights/src/lights_controller_node.cpp b/panther_lights/src/lights_controller_node.cpp
index fb2d461f..d77443ef 100644
--- a/panther_lights/src/lights_controller_node.cpp
+++ b/panther_lights/src/lights_controller_node.cpp
@@ -48,23 +48,23 @@ LightsControllerNode::LightsControllerNode(const rclcpp::NodeOptions & options)
 
   using namespace std::placeholders;
 
-  this->declare_parameter<std::string>("led_config_file");
-  this->declare_parameter<std::string>("user_led_animations_file", "");
+  this->declare_parameter<std::string>("animations_config_path");
+  this->declare_parameter<std::string>("user_led_animations_path", "");
   this->declare_parameter<float>("controller_freq", 50.0);
 
-  const auto led_config_file = this->get_parameter("led_config_file").as_string();
-  const auto user_led_animations_file = this->get_parameter("user_led_animations_file").as_string();
+  const auto animations_config_path = this->get_parameter("animations_config_path").as_string();
+  const auto user_led_animations_path = this->get_parameter("user_led_animations_path").as_string();
   const float controller_freq = this->get_parameter("controller_freq").as_double();
 
-  YAML::Node led_config_desc = YAML::LoadFile(led_config_file);
+  YAML::Node led_config_desc = YAML::LoadFile(animations_config_path);
 
   InitializeLEDPanels(led_config_desc["panels"]);
   InitializeLEDSegments(led_config_desc["segments"], controller_freq);
   InitializeLEDSegmentsMap(led_config_desc["segments_map"]);
   LoadDefaultAnimations(led_config_desc["led_animations"]);
 
-  if (user_led_animations_file != "") {
-    LoadUserAnimations(user_led_animations_file);
+  if (user_led_animations_path != "") {
+    LoadUserAnimations(user_led_animations_path);
   }
 
   segment_converter_ = std::make_shared<SegmentConverter>();
@@ -159,12 +159,12 @@ void LightsControllerNode::LoadDefaultAnimations(const YAML::Node & animations_d
   RCLCPP_INFO(this->get_logger(), "Loaded default animations.");
 }
 
-void LightsControllerNode::LoadUserAnimations(const std::string & user_led_animations_file)
+void LightsControllerNode::LoadUserAnimations(const std::string & user_led_animations_path)
 {
   RCLCPP_DEBUG(this->get_logger(), "Loading user's animations.");
 
   try {
-    YAML::Node user_led_animations = YAML::LoadFile(user_led_animations_file);
+    YAML::Node user_led_animations = YAML::LoadFile(user_led_animations_path);
     auto user_animations = panther_utils::GetYAMLKeyValue<std::vector<YAML::Node>>(
       user_led_animations, "user_animations");
 
diff --git a/panther_lights/test/integration/panther_lights.test.py b/panther_lights/test/integration/panther_lights.test.py
index fafd6bf1..bcc52087 100644
--- a/panther_lights/test/integration/panther_lights.test.py
+++ b/panther_lights/test/integration/panther_lights.test.py
@@ -36,14 +36,14 @@
 
 def generate_test_description():
 
-    led_config_file = (
+    animations_config_path = (
         PathJoinSubstitution([FindPackageShare("panther_lights"), "config", "led_config.yaml"]),
     )
 
     lights_controller_node = Node(
         package="panther_lights",
         executable="lights_controller_node",
-        parameters=[{"led_config_file": led_config_file}],
+        parameters=[{"animations_config_path": animations_config_path}],
     )
 
     lights_driver_node = Node(
diff --git a/panther_lights/test/unit/test_lights_controller_node.cpp b/panther_lights/test/unit/test_lights_controller_node.cpp
index 7a27f3a4..37fcff3b 100644
--- a/panther_lights/test/unit/test_lights_controller_node.cpp
+++ b/panther_lights/test/unit/test_lights_controller_node.cpp
@@ -77,18 +77,18 @@ class TestLightsControllerNode : public testing::Test
   static constexpr char kTestSegmentName[] = "test";
   static constexpr char kTestSegmentLedRange[] = "0-9";
 
-  std::filesystem::path led_config_file_;
+  std::filesystem::path animations_config_path_;
   std::shared_ptr<ControllerNodeWrapper> lights_controller_node_;
 };
 
 TestLightsControllerNode::TestLightsControllerNode()
 {
-  led_config_file_ = testing::TempDir() + "/led_config.yaml";
+  animations_config_path_ = testing::TempDir() + "/led_config.yaml";
 
-  CreateLEDConfig(led_config_file_);
+  CreateLEDConfig(animations_config_path_);
 
   std::vector<rclcpp::Parameter> params;
-  params.push_back(rclcpp::Parameter("led_config_file", led_config_file_));
+  params.push_back(rclcpp::Parameter("animations_config_path", animations_config_path_));
 
   rclcpp::NodeOptions options;
   options.parameter_overrides(params);
@@ -96,7 +96,10 @@ TestLightsControllerNode::TestLightsControllerNode()
   lights_controller_node_ = std::make_shared<ControllerNodeWrapper>(options);
 }
 
-TestLightsControllerNode::~TestLightsControllerNode() { std::filesystem::remove(led_config_file_); }
+TestLightsControllerNode::~TestLightsControllerNode()
+{
+  std::filesystem::remove(animations_config_path_);
+}
 
 void TestLightsControllerNode::CreateLEDConfig(const std::filesystem::path file_path)
 {
diff --git a/panther_manager/CONFIGURATION.md b/panther_manager/CONFIGURATION.md
index 3317e43a..6454c886 100644
--- a/panther_manager/CONFIGURATION.md
+++ b/panther_manager/CONFIGURATION.md
@@ -5,7 +5,7 @@
 For more information regarding shutdown behavior, refer to `ShutdownSingleHost` BT node in the [Actions](#actions) section. An example of a shutdown hosts YAML file can be found below.
 
 ``` yaml
-# My shutdown_hosts_config.yaml
+# My shutdown_hosts.yaml
 hosts:
   # Intel NUC, user computer
   - ip: 10.15.20.3
diff --git a/panther_manager/README.md b/panther_manager/README.md
index f58327f0..718abece 100644
--- a/panther_manager/README.md
+++ b/panther_manager/README.md
@@ -17,7 +17,7 @@ This package contains:
 - [`shutdown.xml`](./behavior_trees/shutdown.xml): BehaviorTree for initiating shutdown procedures.
 - [`lights_manager.yaml`](./config/lights_manager.yaml): Contains parameters for the `lights_manager` node.
 - [`safety_manager.yaml`](./config/safety_manager.yaml): Contains parameters for the `safety_manager` node.
-- [`shutdown_hosts_config.yaml`](./config/shutdown_hosts_config.yaml): List with all hosts to request shutdown.
+- [`shutdown_hosts.yaml`](./config/shutdown_hosts.yaml): List with all hosts to request shutdown.
 
 ## ROS Nodes
 
diff --git a/panther_manager/config/shutdown_hosts_config.yaml b/panther_manager/config/shutdown_hosts.yaml
similarity index 100%
rename from panther_manager/config/shutdown_hosts_config.yaml
rename to panther_manager/config/shutdown_hosts.yaml
diff --git a/panther_manager/launch/manager.launch.py b/panther_manager/launch/manager.launch.py
index 510fb283..0aaf5385 100644
--- a/panther_manager/launch/manager.launch.py
+++ b/panther_manager/launch/manager.launch.py
@@ -62,7 +62,7 @@ def generate_launch_description():
             [
                 FindPackageShare("panther_manager"),
                 "config",
-                "shutdown_hosts_config.yaml",
+                "shutdown_hosts.yaml",
             ]
         ),
         description="Path to file with list of hosts to request shutdown.",

From f6531cf16e4df2f0359ff2d26e485f4cd39d227e Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 4 Oct 2024 10:58:50 +0200
Subject: [PATCH 066/100] New  format of launch args

---
 README.md                                     |  69 ++++----
 ROS_API.md                                    | 153 +++++++++---------
 .../launch/simulate_robot.launch.py           |  13 +-
 panther_localization/README.md                |   2 +-
 ...ea_navsat_params.yaml => nmea_navsat.yaml} |   0
 .../launch/localization.launch.py             |  38 ++---
 .../launch/nmea_navsat.launch.py              |  20 +--
 7 files changed, 146 insertions(+), 149 deletions(-)
 rename panther_localization/config/{nmea_navsat_params.yaml => nmea_navsat.yaml} (100%)

diff --git a/README.md b/README.md
index e887b3cb..7c268f86 100644
--- a/README.md
+++ b/README.md
@@ -83,41 +83,40 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | 🤖      | Available for physical robot |
 | 🖥️      | Available in simulation      |
 
-|     | Argument                     | Description <br/> ***Type:*** `Default`                                                                                                                                                                                                                                                                                                                        |
-| --- | ---------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| 🖥️   | `add_wheel_joints`           | Flag enabling joint_state_publisher to publish information about the wheel position. Should be false when there is a controller that sends this information. <br/> ***bool:*** `False`                                                                                                                                              |
-| 🖥️   | `add_world_transform`        | Adds a world frame that connects the tf trees of individual robots (useful when running multiple robots). <br/> ***bool:*** `False`                                                                                                                                                                                                                                              |
-| 🤖🖥️  | `animations_config_path`    | Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. <br/> ***string:*** [`led_config.yaml`](./panther_lights/config/led_config.yaml)                                                                                                                        |
-| 🖥️   | `battery_config_path`        | Path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only. <br/> ***string:*** `None`                                                                                                                                                                                                           |
-| 🤖🖥️  | `components_config_path`     | Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options). <br/> ***string:*** [`components.yaml`](./panther_description/config/components.yaml)                                          |
-| 🤖🖥️  | `controller_config_path`     | Path to controller configuration file. A path to custom configuration can be specified here. <br/> ***string:*** [`{wheel_type}_controller.yaml`](./panther_controller/config/)                                                                                                                                                                              |
-| 🤖   | `disable_manager`            | Enable or disable manager_bt_node. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                                      |
-| 🤖🖥️  | `fuse_gps`                   | Include GPS for data fusion. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                                            |
-| 🖥️   | `gz_bridge_config_path`      | Path to the parameter_bridge configuration file. <br/> ***string:*** [`gz_bridge.yaml`](./panther_gazebo/config/gz_bridge.yaml)                                                                                                                                                                                                                                 |
-| 🖥️   | `gz_gui`                     | Run simulation with specific GUI layout. <br/> ***string:*** [`teleop.config`](https://github.com/husarion/husarion_gz_worlds/blob/main/config/teleop.config)                                                                                                                                                                                                                |
-| 🖥️   | `gz_headless_mode`           | Run the simulation in headless mode. Useful when a GUI is not needed or to reduce the number of calculations. <br/> ***bool:*** `False`                                                                                                                                                                                             |
-| 🖥️   | `gz_log_level`               | Adjust the level of console output. <br/> ***int:*** `1` (choices: `0`, `1`, `2`, `3`, `4`)                                                                                                                                                                                                                                                                               |
-| 🖥️   | `gz_world`                   | Absolute path to SDF world file. <br/> ***string:*** [`husarion_world.sdf`](https://github.com/husarion/husarion_gz_worlds/blob/main/worlds/husarion_world.sdf)                                                                                                                                                                                                 |
-| 🤖   | `launch_nmea_gps`            | Whether to launch the NMEA NavSat driver node. Advisable when the robot is equipped with the [ANT02](https://husarion.com/manuals/panther/panther-options/#ant02---wi-fi--lte--gps). <br/> ***bool:*** `False`   |
-| 🤖🖥️  | `lights_bt_project_path`     | Path to BehaviorTree project file, responsible for lights management. <br/> ***string:*** [`PantherLightsBT.btproj`](./panther_manager/behavior_trees/PantherLightsBT.btproj)                                                                                                                                                                                |
-| 🤖🖥️  | `localization_config_path`   | Specify the path to the localization configuration file. <br/> ***string:*** [`relative_localization.yaml`](./panther_localization/config/relative_localization.yaml)                                                                                                                                                                                          |
-| 🤖🖥️  | `localization_mode`          | Specifies the localization mode:  <br/>- 'relative' `odometry/filtered` data is relative to the initial position and orientation. <br/>- 'enu' `odometry/filtered` data is relative to initial position and ENU (East North Up) orientation. <br/> ***string:*** `relative` (choices: `relative`, `enu`)                                                    |
-| 🤖🖥️  | `namespace`                  | Add namespace to all launched nodes. <br/> ***string:*** `env(ROBOT_NAMESPACE)`                                                                                                                                                                                                                                                                               |
-| 🤖🖥️  | `publish_robot_state`        | Whether to publish the default Panther robot description. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                |
-| 🖥️   | `robot_model`                 | Specify robot model type. <br/> ***string:*** `env(ROBOT_MODEL)` if not specified `panther` (choices: `lynx`, `panther`)                                                                                                                                                                                                                  |
-| 🤖🖥️  | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`PantherSafetyBT.btproj`](./panther_manager/behavior_trees/PantherSafetyBT.btproj)                                                                                                                                                                   |
-| 🤖🖥️  | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts_config.yaml`](./panther_manager/config/shutdown_hosts_config.yaml)                                                                                                                                                                                                               |
-| 🤖🖥️  | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                                   |
-| 🤖🖥️  | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                                             |
-| 🤖🖥️  | `user_led_animations_path`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                                                             |
-| 🤖🖥️  | `wheel_config_path`          | Path to wheel configuration file. <br/> ***string:*** [`{wheel_type}.yaml`](./panther_description/config)                                                                                                                                                                                                                                                   |
-| 🤖🖥️  | `wheel_type`                 | Specify the wheel type. If the selected wheel type is not 'custom', the wheel_config_path and controller_config_path arguments will be automatically adjusted and can be omitted. <br/> ***string:*** `WH01` (for Panther), `WH05` (for Lynx) (choices: `WH01`, `WH02`, `WH04`, `WH05`, `custom`) |
-| 🖥️   | `x`                          | Initial robot position in the global 'x' axis. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                          |
-| 🖥️   | `y`                          | Initial robot position in the global 'y' axis. <br/> ***float:***` -2.0`                                                                                                                                                                                                                                                                                         |
-| 🖥️   | `z`                          | Initial robot position in the global 'z' axis. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                          |
-| 🖥️   | `roll`                       | Initial robot 'roll' orientation. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                                       |
-| 🖥️   | `pitch`                      | Initial robot 'pitch' orientation. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                                      |
-| 🖥️   | `yaw`                        | Initial robot 'yaw' orientation. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                                        |
+| 🤖   | 🖥️   | Argument                     | Description <br/> ***Type:*** `Default`                                                                                                                                                                                                                                                                            |
+| --- | --- | ---------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| ❌   | ✅   | `add_wheel_joints`           | Flag enabling joint_state_publisher to publish information about the wheel position. Should be false when there is a controller that sends this information. <br/> ***bool:*** `False`                                                                                                                             |
+| ❌   | ✅   | `add_world_transform`        | Adds a world frame that connects the tf trees of individual robots (useful when running multiple robots). <br/> ***bool:*** `False`                                                                                                                                                                                |
+| ✅   | ✅   | `animations_config_path`     | Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. <br/> ***string:*** [`led_config.yaml`](./panther_lights/config/led_config.yaml)                                                                              |
+| ❌   | ✅   | `battery_config_path`        | Path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only. <br/> ***string:*** `None`                                                                                                                                                                |
+| ✅   | ✅   | `components_config_path`     | Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options). <br/> ***string:*** [`components.yaml`](./panther_description/config/components.yaml) |
+| ✅   | ✅   | `controller_config_path`     | Path to controller configuration file. A path to custom configuration can be specified here. <br/> ***string:*** [`{wheel_type}_controller.yaml`](./panther_controller/config/)                                                                                                                                    |
+| ✅   | ✅   | `disable_manager`            | Enable or disable manager_bt_node. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                       |
+| ❌   | ✅   | `gz_bridge_config_path`      | Path to the parameter_bridge configuration file. <br/> ***string:*** [`gz_bridge.yaml`](./panther_gazebo/config/gz_bridge.yaml)                                                                                                                                                                                    |
+| ❌   | ✅   | `gz_gui`                     | Run simulation with specific GUI layout. <br/> ***string:*** [`teleop.config`](https://github.com/husarion/husarion_gz_worlds/blob/main/config/teleop.config)                                                                                                                                                      |
+| ❌   | ✅   | `gz_headless_mode`           | Run the simulation in headless mode. Useful when a GUI is not needed or to reduce the number of calculations. <br/> ***bool:*** `False`                                                                                                                                                                            |
+| ❌   | ✅   | `gz_log_level`               | Adjust the level of console output. <br/> ***int:*** `1` (choices: `0`, `1`, `2`, `3`, `4`)                                                                                                                                                                                                                        |
+| ❌   | ✅   | `gz_world`                   | Absolute path to SDF world file. <br/> ***string:*** [`husarion_world.sdf`](https://github.com/husarion/husarion_gz_worlds/blob/main/worlds/husarion_world.sdf)                                                                                                                                                    |
+| ✅   | ✅   | `lights_bt_project_path`     | Path to BehaviorTree project file, responsible for lights management. <br/> ***string:*** [`PantherLightsBT.btproj`](./panther_manager/behavior_trees/PantherLightsBT.btproj)                                                                                                                                      |
+| ✅   | ✅   | `localization_config_path`   | Specify the path to the localization configuration file. <br/> ***string:*** [`relative_localization.yaml`](./panther_localization/config/relative_localization.yaml)                                                                                                                                              |
+| ✅   | ✅   | `localization_mode`          | Specifies the localization mode:  <br/>- 'relative' `odometry/filtered` data is relative to the initial position and orientation. <br/>- 'enu' `odometry/filtered` data is relative to initial position and ENU (East North Up) orientation. <br/> ***string:*** `relative` (choices: `relative`, `enu`)           |
+| ✅   | ✅   | `namespace`                  | Add namespace to all launched nodes. <br/> ***string:*** `env(ROBOT_NAMESPACE)`                                                                                                                                                                                                                                    |
+| ✅   | ✅   | `publish_robot_state`        | Whether to publish the default Panther robot description. <br/> ***bool:*** `True`                                                                                                                                                                                                                                 |
+| ❌   | ✅   | `robot_model`                | Specify robot model type. <br/> ***string:*** `env(ROBOT_MODEL)` if not specified `panther` (choices: `lynx`, `panther`)                                                                                                                                                                                           |
+| ✅   | ✅   | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`PantherSafetyBT.btproj`](./panther_manager/behavior_trees/PantherSafetyBT.btproj)                                                                                                                         |
+| ✅   | ✅   | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts_config.yaml`](./panther_manager/config/shutdown_hosts_config.yaml)                                                                                                                                                       |
+| ✅   | ✅   | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                    |
+| ✅   | ✅   | `use_gps`                    | Whether to launch the NMEA NavSat driver node. Advisable when the robot is equipped with the [ANT02](https://husarion.com/manuals/panther/panther-options/#ant02---wi-fi--lte--gps). <br/> ***bool:*** `False`                                                                                                     |
+| ✅   | ✅   | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                              |
+| ✅   | ✅   | `user_led_animations_path`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                    |
+| ✅   | ✅   | `wheel_config_path`          | Path to wheel configuration file. <br/> ***string:*** [`{wheel_type}.yaml`](./panther_description/config)                                                                                                                                                                                                          |
+| ✅   | ✅   | `wheel_type`                 | Specify the wheel type. If the selected wheel type is not 'custom', the wheel_config_path and controller_config_path arguments will be automatically adjusted and can be omitted. <br/> ***string:*** `WH01` (for Panther), `WH05` (for Lynx) (choices: `WH01`, `WH02`, `WH04`, `WH05`, `custom`)                  |
+| ❌   | ✅   | `x`                          | Initial robot position in the global 'x' axis. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                            |
+| ❌   | ✅   | `y`                          | Initial robot position in the global 'y' axis. <br/> ***float:***` -2.0`                                                                                                                                                                                                                                           |
+| ❌   | ✅   | `z`                          | Initial robot position in the global 'z' axis. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                            |
+| ❌   | ✅   | `roll`                       | Initial robot 'roll' orientation. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                         |
+| ❌   | ✅   | `pitch`                      | Initial robot 'pitch' orientation. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                        |
+| ❌   | ✅   | `yaw`                        | Initial robot 'yaw' orientation. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                          |
 
 > [!TIP]
 >
diff --git a/ROS_API.md b/ROS_API.md
index e7fb79a6..2685fb45 100644
--- a/ROS_API.md
+++ b/ROS_API.md
@@ -37,91 +37,92 @@ Below is information about the physical robot API. For the simulation, topics an
 | 🤖      | Available for physical robot    |
 | 🖥️      | Available in simulation         |
 | ⚙️      | Requires specific configuration |
+<!-- ⚙️ - in future will be default and can be removed -->
 
 ### Nodes
 
-|     | Node name                 | Description                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                           |
-| --- | ------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| 🤖   | `battery_driver`          | Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier versions of the robot. <br/> [*panther_batter/battery_driver_node*](./panther_battery/include/panther_battery/battery_driver_node.hpp)                                                                                                                                                                                                                                                                              |
-| 🤖🖥️  | `controller_manager`      | The Controller Manager performs two main functions. First, it manages controllers and their required interfaces, handling tasks like loading, activating, deactivating, and unloading. Second, it interacts with hardware components, ensuring access to their interfaces. This node manages the: `imu_broadcaster`, `joint_state_broadcaster`, `drive_controller`. <br/> *[controller_manager/controller_manager](https://github.com/ros-controls/ros2_control/blob/master/controller_manager)*                                      |
-| 🤖🖥️  | `drive_controller`        | Manages mobile robots with a differential or mecanum drive depending on the configuration. It converts velocity commands for the robot body into wheel commands for the base. It also calculates odometry from hardware feedback and shares it. <br/> *[diff_drive_controller/diff_drive_controller](https://github.com/ros-controls/ros2_controllers/tree/master/diff_drive_controller) or [mecanum_drive_controller/mecanum_drive_controller](https://github.com/husarion/husarion_controllers/tree/main/mecanum_drive_controller)* |
-| 🤖🖥️  | `ekf_filter`              | The Extended Kalman Filter node is designed to fuse odometry data from various sources, including wheel encoders, IMU, and GPS. <br/> *[robot_localization/ekf_filter](https://github.com/cra-ros-pkg/robot_localization)*                                                                                                                                                                                                                                                                                                            |
-| 🤖   | `hardware_controller`     | Plugin responsible for communicating with engine controllers via the CAN bus and providing E-Stop functionalities. <br/> *[panther_hardware_interfaces/PantherSystem](./panther_hardware_interfaces/src/panther_system/)*                                                                                                                                                                                                                                                                                                             |
-| 🤖   | `gps`                     | Node responsible for parsing NMEA strings and publishing standard ROS NavSat message types. <br/> *[nmea_navsat_driver/nmea_navsat_driver](https://github.com/ros-drivers/nmea_navsat_driver/tree/ros2/src/libnmea_navsat_driver)*                                                                                                                                                                                                                                                                                                                                 |
-| 🖥️   | `gz_ros2_control`         | Responsible for integrating the ros2_control controller architecture with the Gazebo simulator. <br/> [gz_ros2_control/gz_ros2_control](https://github.com/ros-controls/gz_ros2_control/tree/master/gz_ros2_control/src)                                                                                                                                                                                                                                                                                                              |
-| 🖥️   | `gz_estop_gui`            | The node is part of the Gazebo GUI plugin, enabling easy E-stop state modifications directly within the simulation. <br/> [panther_gazebo/EStop](./panther_gazebo/src/gui/e_stop.cpp)                                                                                                                                                                                                                                                                                |
-| 🤖🖥️  | `imu_broadcaster`         | Publishes readings of IMU sensors. <br/> *[imu_sensor_broadcaster/imu_sensor_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/imu_sensor_broadcaster)*                                                                                                                                                                                                                                                                                                                                                       |
-| 🤖🖥️  | `joint_state_broadcaster` | Reads all state interfaces and reports them on specific topics. <br/> *[joint_state_broadcaster/joint_state_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)*                                                                                                                                                                                                                                                                                                                       |
-| 🤖🖥️  | `lights_container`        | Node for managing ROS components. This node manages: `lights_controller`, `lights_driver`. <br/>[*rclcpp_components/component_container*](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_components)                                                                                                                                                                                                                                                                                                                              |
-| 🤖🖥️  | `lights_controller`       | This node is responsible for processing animations and publishing frames to be displayed on the Husarion Panther robot Bumper Lights. <br/> [*panther_lights/LightsControllerNode*](./panther_lights/include/panther_lights/lights_controller_node.hpp)                                                                                                                                                                                                                                                                                                  |
-| 🤖   | `lights_driver`           | This node is responsible for displaying frames on the Husarion Panther robot's Bumper Lights. <br/> [*panther_lights/LightsDriverNode*](./panther_lights/include/panther_lights/lights_driver_node.hpp)                                                                                                                                                                                                                                                                                                                                                  |
-| 🤖🖥️  | `lights_manager`          | Node responsible for managing Bumper Lights animation scheduling. <br/> [panther_manager/lights_manager](./panther_manager/include/panther_manager/lights_manager_node.hpp)                                                                                                                                                                                                                                                                                                                                                                               |
-| 🤖🖥️⚙️ | `navsat_transform`        | It converts raw GPS data into odometry data and publishes corrected GPS positions based on sensor data at a higher frequency. <br/> *[robot_localization/navsat_transform](https://github.com/cra-ros-pkg/robot_localization)*                                                                                                                                                                                                                                                                                                        |
-| 🖥️   | `panther_base_gz_bridge`  | Convert and transmit data between ROS and Gazebo <br/> *[ros_gz_bridge/parameter_bridge](https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge)*                                                                                                                                                                                                                                                                                                                                                                                |
-| 🤖🖥️  | `robot_state_publisher`   | Broadcasts a robot's state to tf2 using a provided URDF model and joint states. It updates the model and broadcasts poses for fixed and movable joints to tf2 topics. <br/> *[robot_state_publisher/robot_state_publisher](https://github.com/ros/robot_state_publisher)*                                                                                                                                                                                                                                                             |
-| 🤖   | `safety_manager`          | Node responsible for managing safety features, and software shutdown of components. <br/> *[panther_manager/safety_manager_node](./panther_manager/include/panther_manager/safety_manager_node.hpp)*                                                                                                                                                                                                                                                                                                                                                      |
-| 🤖   | `system_monitor`          | Publishes system state of the Built-in Computer such as CPU usage, RAM memory usage, disk usage and  CPU temperature. <br/> *[panther_diagnostic/system_monitor_node](./panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp)*                                                                                                                                                                                                                                                                                                             |
+| 🤖   | 🖥️   | Node name                 | Description                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                           |
+| --- | --- | ------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| ✅   | ❌   | `battery_driver`          | Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier versions of the robot. <br/> [_panther_batter/battery_driver_node_](./panther_battery/include/panther_battery/battery_driver_node.hpp)                                                                                                                                                                                                                                                          |
+| ✅   | ✅   | `controller_manager`      | The Controller Manager performs two main functions. First, it manages controllers and their required interfaces, handling tasks like loading, activating, deactivating, and unloading. Second, it interacts with hardware components, ensuring access to their interfaces. This node manages the: `imu_broadcaster`, `joint_state_broadcaster`, `drive_controller`. <br/> _[controller_manager/controller_manager](https://github.com/ros-controls/ros2_control/blob/master/controller_manager)_                                      |
+| ✅   | ✅   | `drive_controller`        | Manages mobile robots with a differential or mecanum drive depending on the configuration. It converts velocity commands for the robot body into wheel commands for the base. It also calculates odometry from hardware feedback and shares it. <br/> _[diff_drive_controller/diff_drive_controller](https://github.com/ros-controls/ros2_controllers/tree/master/diff_drive_controller) or [mecanum_drive_controller/mecanum_drive_controller](https://github.com/husarion/husarion_controllers/tree/main/mecanum_drive_controller)_ |
+| ✅   | ✅   | `ekf_filter`              | The Extended Kalman Filter node is designed to fuse odometry data from various sources, including wheel encoders, IMU, and GPS. <br/> _[robot_localization/ekf_filter](https://github.com/cra-ros-pkg/robot_localization)_                                                                                                                                                                                                                                                                                                            |
+| ✅   | ❌   | `gps`                     | Node responsible for parsing NMEA strings and publishing standard ROS NavSat message types. <br/> _[nmea_navsat_driver/nmea_navsat_driver](https://github.com/ros-drivers/nmea_navsat_driver/tree/ros2/src/libnmea_navsat_driver)_                                                                                                                                                                                                                                                                                                    |
+| ❌   | ✅   | `gz_ros2_control`         | Responsible for integrating the ros2_control controller architecture with the Gazebo simulator. <br/> [gz_ros2_control/gz_ros2_control](https://github.com/ros-controls/gz_ros2_control/tree/master/gz_ros2_control/src)                                                                                                                                                                                                                                                                                                              |
+| ❌   | ✅   | `gz_estop_gui`            | The node is part of the Gazebo GUI plugin, enabling easy E-stop state modifications directly within the simulation. <br/> [panther_gazebo/EStop](./panther_gazebo/src/gui/e_stop.cpp)                                                                                                                                                                                                                                                                                                                                                 |
+| ✅   | ❌   | `hardware_controller`     | Plugin responsible for communicating with engine controllers via the CAN bus and providing E-Stop functionalities. <br/> _[panther_hardware_interfaces/PantherSystem](./panther_hardware_interfaces/src/panther_system/)_                                                                                                                                                                                                                                                                                                             |
+| ✅   | ✅   | `imu_broadcaster`         | Publishes readings of IMU sensors. <br/> _[imu_sensor_broadcaster/imu_sensor_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/imu_sensor_broadcaster)_                                                                                                                                                                                                                                                                                                                                                       |
+| ✅   | ✅   | `joint_state_broadcaster` | Reads all state interfaces and reports them on specific topics. <br/> _[joint_state_broadcaster/joint_state_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)_                                                                                                                                                                                                                                                                                                                       |
+| ✅   | ✅   | `lights_container`        | Node for managing ROS components. This node manages: `lights_controller`, `lights_driver`. <br/>[_rclcpp_components/component_container_](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_components)                                                                                                                                                                                                                                                                                                                              |
+| ✅   | ✅   | `lights_controller`       | This node is responsible for processing animations and publishing frames to be displayed on the Husarion Panther robot Bumper Lights. <br/> [_panther_lights/LightsControllerNode_](./panther_lights/include/panther_lights/lights_controller_node.hpp)                                                                                                                                                                                                                                                                               |
+| ✅   | ❌   | `lights_driver`           | This node is responsible for displaying frames on the Husarion Panther robot's Bumper Lights. <br/> [_panther_lights/LightsDriverNode_](./panther_lights/include/panther_lights/lights_driver_node.hpp)                                                                                                                                                                                                                                                                                                                               |
+| ✅   | ✅   | `lights_manager`          | Node responsible for managing Bumper Lights animation scheduling. <br/> [panther_manager/lights_manager](./panther_manager/include/panther_manager/lights_manager_node.hpp)                                                                                                                                                                                                                                                                                                                                                           |
+| ✅   | ✅   | ⚙️ `navsat_transform` ⚙️        | It converts raw GPS data into odometry data and publishes corrected GPS positions based on sensor data at a higher frequency. Requires `launch_nmea_gps` launch argument. <br/> _[robot_localization/navsat_transform](https://github.com/cra-ros-pkg/robot_localization)_                                                                                                                                                                                                                                                                                                        |
+| ❌   | ✅   | `panther_base_gz_bridge`  | Convert and transmit data between ROS and Gazebo. <br/> _[ros_gz_bridge/parameter_bridge](https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge)_                                                                                                                                                                                                                                                                                                                                                                               |
+| ✅   | ✅   | `robot_state_publisher`   | Broadcasts a robot's state to tf2 using a provided URDF model and joint states. It updates the model and broadcasts poses for fixed and movable joints to tf2 topics. <br/> _[robot_state_publisher/robot_state_publisher](https://github.com/ros/robot_state_publisher)_                                                                                                                                                                                                                                                             |
+| ✅   | ❌   | `safety_manager`          | Node responsible for managing safety features, and software shutdown of components. <br/> _[panther_manager/safety_manager_node](./panther_manager/include/panther_manager/safety_manager_node.hpp)_                                                                                                                                                                                                                                                                                                                                  |
+| ✅   | ❌   | `system_monitor`          | Publishes system state of the Built-in Computer such as CPU usage, RAM memory usage, disk usage and CPU temperature. <br/> _[panther_diagnostic/system_monitor_node](./panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp)_                                                                                                                                                                                                                                                                                      |
 
 ### Topics
 
-|     | Topic                              | Description                                                                                                                                                                                                                                                                                                                                                               |
-| --- | ---------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| 🤖🖥️  | `battery/battery_status`           | Mean values of both batteries if Panther has two batteries. Otherwise, the state of the single battery will be published.<br/> [*sensor_msgs/BatteryState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html)                                                                                                                                           |
-| 🤖   | `battery/charging_status`          | Battery charging status value.<br/> [*panther_msgs/ChargingStatus*](https://github.com/husarion/panther_msgs)                                                                                                                                                                                                                                                             |
-| 🤖🖥️  | `cmd_vel`                          | Command velocity value.<br/> [*geometry_msgs/Twist*](https://docs.ros2.org/latest/api/geometry_msgs/msg/Twist.html)                                                                                                                                                                                                                                                       |
-| 🤖🖥️  | `diagnostics`                      | Diagnostic data.<br/> [*diagnostic_msgs/DiagnosticArray*](https://docs.ros2.org/latest/api/diagnostic_msgs/msg/DiagnosticArray.html)                                                                                                                                                                                                                                      |
-| 🤖🖥️  | `dynamic_joint_states`             | Information about the state of various movable joints in a robotic system.<br/> [*control_msgs/DynamicJointState*](https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/DynamicJointState.msg)                                                                                                                                                       |
-| 🤖🖥️⚙️ | `gps/filtered`                     | Filtered GPS position after fusing odometry data.<br/> [*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html)                                                                                                                                                                                                                         |
-| 🤖🖥️⚙️ | `gps/fix`                          | Raw GPS data.<br/> [*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html)                                                                                                                                                                                                                                                             |
-| 🤖   | `gps/time_reference`                | The timestamp from the GPS device.<br/> [*sensor_msgs/TimeReference*](https://docs.ros2.org/latest/api/sensor_msgs/msg/TimeReference.html) |
-| 🤖   | `gps/vel`                           | Velocity output from the GPS device.<br/> [*geometry_msgs/TwistStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/TwistStamped.html) |
-| 🤖🖥️  | `hardware/e_stop`                  | Current E-stop state.<br/> [*std_msgs/Bool*](https://docs.ros2.org/latest/api/std_msgs/msg/Bool.html).                                                                                                                                                                                                                                                               |
-| 🤖   | `hardware/io_state`                | Current IO state.<br/> [*panther_msgs/IOState*](https://github.com/husarion/panther_msgs)                                                                                                                                                                                                                                                                                 |
-| 🤖🖥️  | `hardware/robot_driver_state` | Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.<br/> [*panther_msgs/RobotDriverState*](https://github.com/husarion/panther_msgs)                                                                                                                                                                                         |
-| 🤖🖥️  | `imu/data`                         | Filtered IMU data.<br/> [*sensor_msgs/Imu*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)                                                                                                                                                                                                                                                                    |
-| 🤖🖥️  | `joint_states`                     | Provides information about the state of various joints in a robotic system.<br/> [*sensor_msgs/JointState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/JointState.html)                                                                                                                                                                                             |
-| 🤖🖥️  | `lights/channel_1_frame`           | Frame to be displayed on robot Front Bumper Lights.<br/> [*sensor_msgs/Image*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html)                                                                                                                                                                                                                               |
-| 🤖🖥️  | `lights/channel_2_frame`           | Frame to be displayed on robot Rear Bumper Lights.<br/> [*sensor_msgs/Image*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html)                                                                                                                                                                                                                                |
-| 🤖🖥️  | `localization/set_pose`            | Sets the pose of the EKF node.<br/> [*geometry_msgs/PoseWithCovarianceStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/PoseWithCovarianceStamped.html)                                                                                                                                                                                                        |
-| 🤖🖥️  | `odometry/filtered`                | Contains information about the filtered position and orientation. When `localization_mode` is `relative`, the position and orientation are relative to the starting point. When `localization_mode` is `enu`, the orientation is relative to the east-north-up (ENU) coordinates.<br/> [*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) |
-| 🤖🖥️  | `odometry/wheels`                  | Robot odometry calculated from wheels.<br/> [*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html)                                                                                                                                                                                                                                            |
-| 🤖🖥️  | `robot_description`                | Contains information about robot description from URDF file. <br/> [*std_msgs/String*](https://docs.ros2.org/latest/api/std_msgs/msg/String.html)                                                                                                                                                                                                                         |
-| 🤖   | `system_status`                    | State of the system, including Built-in Computer's CPU temperature and load. <br/> [*panther_msgs/SystemStatus*](https://github.com/husarion/panther_msgs)                                                                                                                                                                                                                |
-| 🤖🖥️  | `tf`                               | Transforms of robot system.<br/> [*tf2_msgs/TFMessage*](https://docs.ros2.org/latest/api/tf2_msgs/msg/TFMessage.html)                                                                                                                                                                                                                                                     |
-| 🤖🖥️  | `tf_static`                        | Static transforms of robot system.<br/> [*tf2_msgs/TFMessage*](https://docs.ros2.org/latest/api/tf2_msgs/msg/TFMessage.html)                                                                                                                                                                                                                                              |
+| 🤖   | 🖥️   | Topic                         | Description                                                                                                                                                                                                                                                                                                                                                               |
+| --- | --- | ----------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| ✅   | ✅   | `battery/battery_status`      | Mean values of both batteries if Panther has two batteries. Otherwise, the state of the single battery will be published. <br/> [_sensor_msgs/BatteryState_](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html)                                                                                                                                           |
+| ✅   | ❌   | `battery/charging_status`     | Battery charging status value. <br/> [*panther_msgs/ChargingStatus*](https://github.com/husarion/panther_msgs)                                                                                                                                                                                                                                                             |
+| ✅   | ✅   | `cmd_vel`                     | Command velocity value. <br/> [_geometry_msgs/Twist_](https://docs.ros2.org/latest/api/geometry_msgs/msg/Twist.html)                                                                                                                                                                                                                                                       |
+| ✅   | ✅   | `diagnostics`                 | Diagnostic data. <br/> [_diagnostic_msgs/DiagnosticArray_](https://docs.ros2.org/latest/api/diagnostic_msgs/msg/DiagnosticArray.html)                                                                                                                                                                                                                                      |
+| ✅   | ✅   | `dynamic_joint_states`        | Information about the state of various movable joints in a robotic system. <br/> [_control_msgs/DynamicJointState_](https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/DynamicJointState.msg)                                                                                                                                                       |
+| ✅   | ✅   | ⚙️ `gps/filtered` ⚙️                | Filtered GPS position after fusing odometry data. <br/> [_sensor_msgs/NavSatFix_](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html)                                                                                                                                                                                                                         |
+| ✅   | ✅   | ⚙️ `gps/fix` ⚙️                     | Raw GPS data. <br/> [_sensor_msgs/NavSatFix_](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html)                                                                                                                                                                                                                                                             |
+| ✅   | ❌   | `gps/time_reference`          | The timestamp from the GPS device. <br/> [_sensor_msgs/TimeReference_](https://docs.ros2.org/latest/api/sensor_msgs/msg/TimeReference.html)                                                                                                                                                                                                                                |
+| ✅   | ❌   | `gps/vel`                     | Velocity output from the GPS device. <br/> [_geometry_msgs/TwistStamped_](https://docs.ros2.org/latest/api/geometry_msgs/msg/TwistStamped.html)                                                                                                                                                                                                                            |
+| ✅   | ✅   | `hardware/e_stop`             | Current E-stop state. <br/> [_std_msgs/Bool_](https://docs.ros2.org/latest/api/std_msgs/msg/Bool.html).                                                                                                                                                                                                                                                                    |
+| ✅   | ❌   | `hardware/io_state`           | Current IO state. <br/> [_panther_msgs/IOState_](https://github.com/husarion/panther_msgs)                                                                                                                                                                                                                                                                                 |
+| ✅   | ✅   | `hardware/robot_driver_state` | Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data. <br/> [_panther_msgs/RobotDriverState_](https://github.com/husarion/panther_msgs)                                                                                                                                                                                    |
+| ✅   | ✅   | `imu/data`                    | Filtered IMU data. <br/> [_sensor_msgs/Imu_](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)                                                                                                                                                                                                                                                                    |
+| ✅   | ✅   | `joint_states`                | Provides information about the state of various joints in a robotic system. <br/> [_sensor_msgs/JointState_](https://docs.ros2.org/latest/api/sensor_msgs/msg/JointState.html)                                                                                                                                                                                             |
+| ✅   | ✅   | `lights/channel_1_frame`      | Frame to be displayed on robot Front Bumper Lights. <br/> [_sensor_msgs/Image_](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html)                                                                                                                                                                                                                               |
+| ✅   | ✅   | `lights/channel_2_frame`      | Frame to be displayed on robot Rear Bumper Lights. <br/> [_sensor_msgs/Image_](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html)                                                                                                                                                                                                                                |
+| ✅   | ✅   | `localization/set_pose`       | Sets the pose of the EKF node. <br/> [_geometry_msgs/PoseWithCovarianceStamped_](https://docs.ros2.org/latest/api/geometry_msgs/msg/PoseWithCovarianceStamped.html)                                                                                                                                                                                                        |
+| ✅   | ✅   | `odometry/filtered`           | Contains information about the filtered position and orientation. When `localization_mode` is `relative`, the position and orientation are relative to the starting point. When `localization_mode` is `enu`, the orientation is relative to the east-north-up (ENU) coordinates. <br/> [_nav_msgs/Odometry_](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) |
+| ✅   | ✅   | `odometry/wheels`             | Robot odometry calculated from wheels. <br/> [_nav_msgs/Odometry_](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html)                                                                                                                                                                                                                                            |
+| ✅   | ✅   | `robot_description`           | Contains information about robot description from URDF file. <br/> [_std_msgs/String_](https://docs.ros2.org/latest/api/std_msgs/msg/String.html)                                                                                                                                                                                                                         |
+| ✅   | ❌   | `system_status`               | State of the system, including Built-in Computer's CPU temperature and load. <br/> [_panther_msgs/SystemStatus_](https://github.com/husarion/panther_msgs)                                                                                                                                                                                                                |
+| ✅   | ✅   | `tf`                          | Transforms of robot system. <br/> [_tf2_msgs/TFMessage_](https://docs.ros2.org/latest/api/tf2_msgs/msg/TFMessage.html)                                                                                                                                                                                                                                                     |
+| ✅   | ✅   | `tf_static`                   | Static transforms of robot system. <br/> [_tf2_msgs/TFMessage_](https://docs.ros2.org/latest/api/tf2_msgs/msg/TFMessage.html)                                                                                                                                                                                                                                              |
 
 #### Hidden topics
 
-|     | Topic                           | Description                                                                                                                                                           |
-| --- | ------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| 🤖   | `_battery/battery_1_status_raw` | First battery raw state.<br/> [*sensor_msgs/BatteryState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html)                                        |
-| 🤖   | `_battery/battery_2_status_raw` | Second battery raw state. Published if second battery detected.<br/> [*sensor_msgs/BatteryState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) |
-| 🤖   | `_gps/heading`                       | Not supported for current configuration.<br/> [*geometry_msgs/QuaternionStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/QuaternionStamped.html)|
-| 🤖🖥️⚙️ | `_odometry/gps`                 | Transformed raw GPS data to odometry format.<br/> [*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html)                                  |
+| 🤖   | 🖥️   | Topic                           | Description                                                                                                                                                           |
+| --- | --- | ------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| ✅   | ❌   | `_battery/battery_1_status_raw` | First battery raw state. <br/> [_sensor_msgs/BatteryState_](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html)                                        |
+| ✅   | ❌   | `_battery/battery_2_status_raw` | Second battery raw state. Published if second battery detected. <br/> [_sensor_msgs/BatteryState_](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) |
+| ✅   | ❌   | `_gps/heading`                  | Not supported for current configuration. <br/> [_geometry_msgs/QuaternionStamped_](https://docs.ros2.org/latest/api/geometry_msgs/msg/QuaternionStamped.html)          |
+| ✅ | ✅   | ⚙️ `_odometry/gps` ⚙️                 | Transformed raw GPS data to odometry format. <br/> [_nav_msgs/Odometry_](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html)                                  |
 
 ### Services
 
-|     | Service                                           | Description                                                                                                                                                                                            |
-| --- | ------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
-| 🤖🖥️  | `controller_manager/configure_controller`         | Manage lifecycle transition.<br/> [controller_manager_msgs/srv/ConfigureController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)                                  |
-| 🤖🖥️  | `controller_manager/list_controller_types`        | Output the available controller types and their base classes.<br/> [controller_manager_msgs/srv/ListControllerTypes](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) |
-| 🤖🖥️  | `controller_manager/list_controllers`             | Output the list of loaded controllers, their type and status.<br/> [controller_manager_msgs/srv/ListControllers](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)     |
-| 🤖🖥️  | `controller_manager/list_hardware_components`     | Output the list of available hardware components.<br/> [controller_manager_msgs/srv/ListHardwareComponents](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)          |
-| 🤖🖥️  | `controller_manager/list_hardware_interfaces`     | Output the list of available command and state interfaces.<br/> [controller_manager_msgs/srv/ListHardwareInterfaces](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) |
-| 🤖🖥️  | `controller_manager/load_controller`              | Load a controller in a controller manager.<br/> [controller_manager_msgs/srv/LoadController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)                         |
-| 🤖🖥️  | `controller_manager/reload_controller_libraries`  | Reload controller libraries.<br/> [controller_manager_msgs/srv/ReloadControllerLibraries](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)                            |
-| 🤖🖥️  | `controller_manager/set_hardware_component_state` | Adjust the state of the hardware component.<br/> [controller_manager_msgs/srv/SetHardwareComponentState](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)             |
-| 🤖🖥️  | `controller_manager/switch_controller`            | Switch controllers in a controller manager.<br/> [controller_manager_msgs/srv/SwitchController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)                      |
-| 🤖🖥️  | `controller_manager/unload_controller`            | Unload a controller in a controller manager.<br/> [controller_manager_msgs/srv/UnloadController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)                     |
-| 🤖   | `hardware/aux_power_enable`                       | Enables or disables AUX power.<br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                                 |
-| 🤖   | `hardware/charger_enable`                         | Enables or disables external charger.<br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                          |
-| 🤖   | `hardware/digital_power_enable`                   | Enables or disables digital power.<br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                             |
-| 🤖🖥️  | `hardware/e_stop_reset`                           | Resets E-stop.<br/> [std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html)                                                                                                 |
-| 🤖🖥️  | `hardware/e_stop_trigger`                         | Triggers E-stop.<br/> [std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html)                                                                                               |
-| 🤖   | `hardware/fan_enable`                             | Enables or disables fan.<br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                                       |
-| 🤖   | `hardware/motor_power_enable`                     | Enables or disables motor power.<br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                               |
-| 🤖🖥️  | `lights/set_animation`                            | Sets LED animation.<br/> [panther_msgs/srv/SetLEDAnimation](https://github.com/husarion/panther_msgs)                                                                                                  |
-| 🤖   | `lights/set_brightness`                           | Sets global LED brightness, value ranges from **0.0** to **1.0**.<br/> [panther_msgs/SetLEDBrightness](https://github.com/husarion/panther_msgs)                                                       |
-| 🤖🖥️  | `localization/enable`                             | Enable EKF node.<br/> [std_srvs/srv/Empty](https://docs.ros2.org/latest/api/std_srvs/srv/Empty.html)                                                                                                   |
-| 🤖🖥️  | `localization/set_pose`                           | Set pose of EKF node.<br/> [robot_localization/srv/SetPose](https://github.com/cra-ros-pkg/robot_localization/tree/ros2)                                                                               |
-| 🤖🖥️  | `localization/toggle`                             | Toggle filter processing in the EKF node.<br/> [robot_localization/srv/ToggleFilterProcessing](https://github.com/cra-ros-pkg/robot_localization/tree/ros2)                                            |
+| 🤖   | 🖥️   | Service                                           | Description                                                                                                                                                                                            |
+| --- | --- | ------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| ✅   | ✅   | `controller_manager/configure_controller`         | Manage lifecycle transition. <br/> [controller_manager_msgs/srv/ConfigureController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)                                  |
+| ✅   | ✅   | `controller_manager/list_controller_types`        | Output the available controller types and their base classes. <br/> [controller_manager_msgs/srv/ListControllerTypes](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) |
+| ✅   | ✅   | `controller_manager/list_controllers`             | Output the list of loaded controllers, their type and status. <br/> [controller_manager_msgs/srv/ListControllers](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)     |
+| ✅   | ✅   | `controller_manager/list_hardware_components`     | Output the list of available hardware components. <br/> [controller_manager_msgs/srv/ListHardwareComponents](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)          |
+| ✅   | ✅   | `controller_manager/list_hardware_interfaces`     | Output the list of available command and state interfaces. <br/> [controller_manager_msgs/srv/ListHardwareInterfaces](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) |
+| ✅   | ✅   | `controller_manager/load_controller`              | Load a controller in a controller manager. <br/> [controller_manager_msgs/srv/LoadController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)                         |
+| ✅   | ✅   | `controller_manager/reload_controller_libraries`  | Reload controller libraries. <br/> [controller_manager_msgs/srv/ReloadControllerLibraries](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)                            |
+| ✅   | ✅   | `controller_manager/set_hardware_component_state` | Adjust the state of the hardware component. <br/> [controller_manager_msgs/srv/SetHardwareComponentState](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)             |
+| ✅   | ✅   | `controller_manager/switch_controller`            | Switch controllers in a controller manager. <br/> [controller_manager_msgs/srv/SwitchController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)                      |
+| ✅   | ✅   | `controller_manager/unload_controller`            | Unload a controller in a controller manager. <br/> [controller_manager_msgs/srv/UnloadController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)                     |
+| ✅   | ❌   | `hardware/aux_power_enable`                       | Enables or disables AUX power. <br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                                 |
+| ✅   | ❌   | `hardware/charger_enable`                         | Enables or disables external charger. <br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                          |
+| ✅   | ❌   | `hardware/digital_power_enable`                   | Enables or disables digital power. <br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                             |
+| ✅   | ✅   | `hardware/e_stop_reset`                           | Resets E-stop. <br/> [std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html)                                                                                                 |
+| ✅   | ✅   | `hardware/e_stop_trigger`                         | Triggers E-stop. <br/> [std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html)                                                                                               |
+| ✅   | ❌   | `hardware/fan_enable`                             | Enables or disables fan. <br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                                       |
+| ✅   | ❌   | `hardware/motor_power_enable`                     | Enables or disables motor power. <br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                               |
+| ✅   | ✅   | `lights/set_animation`                            | Sets LED animation. <br/> [panther_msgs/srv/SetLEDAnimation](https://github.com/husarion/panther_msgs)                                                                                                  |
+| ✅   | ✅   | `localization/enable`                             | Enable EKF node. <br/> [std_srvs/srv/Empty](https://docs.ros2.org/latest/api/std_srvs/srv/Empty.html)                                                                                                   |
+| ✅   | ❌   | `lights/set_brightness`                           | Sets global LED brightness, value ranges from **0.0** to **1.0**. <br/> [panther_msgs/SetLEDBrightness](https://github.com/husarion/panther_msgs)                                                       |
+| ✅   | ✅   | `localization/set_pose`                           | Set pose of EKF node. <br/> [robot_localization/srv/SetPose](https://github.com/cra-ros-pkg/robot_localization/tree/ros2)                                                                               |
+| ✅   | ✅   | `localization/toggle`                             | Toggle filter processing in the EKF node. <br/> [robot_localization/srv/ToggleFilterProcessing](https://github.com/cra-ros-pkg/robot_localization/tree/ros2)                                            |
diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py
index 2bd4f822..edec27f2 100644
--- a/panther_gazebo/launch/simulate_robot.launch.py
+++ b/panther_gazebo/launch/simulate_robot.launch.py
@@ -18,7 +18,7 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.conditions import IfCondition
+from launch.conditions import IfCondition, UnlessCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
     EnvironmentVariable,
@@ -32,7 +32,6 @@
 
 
 def generate_launch_description():
-
     add_world_transform = LaunchConfiguration("add_world_transform")
     declare_add_world_transform_arg = DeclareLaunchArgument(
         "add_world_transform",
@@ -71,6 +70,14 @@ def generate_launch_description():
         ),
     )
 
+    disable_manager = LaunchConfiguration("disable_manager")
+    declare_disable_manager_arg = DeclareLaunchArgument(
+        "disable_manager",
+        default_value="False",
+        description="Enable or disable manager_bt_node.",
+        choices=["True", "true", "False", "false"],
+    )
+
     gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path")
     declare_gz_bridge_config_path_arg = DeclareLaunchArgument(
         "gz_bridge_config_path",
@@ -132,6 +139,7 @@ def generate_launch_description():
             )
         ),
         launch_arguments={"namespace": namespace, "use_sim": "True"}.items(),
+        condition=UnlessCondition(disable_manager),
     )
 
     controller_launch = IncludeLaunchDescription(
@@ -232,6 +240,7 @@ def generate_launch_description():
         declare_battery_config_path_arg,
         declare_robot_model_arg,  # robot_model is used by components_config_path
         declare_components_config_path_arg,
+        declare_disable_manager_arg,
         declare_gz_bridge_config_path_arg,
         declare_namespace_arg,
         declare_use_ekf_arg,
diff --git a/panther_localization/README.md b/panther_localization/README.md
index 416af897..6dc52763 100644
--- a/panther_localization/README.md
+++ b/panther_localization/README.md
@@ -13,6 +13,6 @@ This package contains:
 
 - [`enu_localization.yaml`](./config/enu_localization.yaml): configures data fusion for `ekf_filter` and `navsat_transform` nodes, using **wheel encoders** and **IMU**. Orientation follows East-North-Up (ENU) coordinates.
 - [`enu_localization_with_gps.yaml`](./config/enu_localization_with_gps.yaml): configures data fusion for `ekf_filter` and `navsat_transform` nodes, using **wheel encoders**, **IMU**, and **GPS**. Orientation follows East-North-Up (ENU) coordinates.
-- [`nmea_navsat_params.yaml`](./config/nmea_navsat_params.yaml): contains parameters for NMEA NavSat driver node.
+- [`nmea_navsat.yaml`](./config/nmea_navsat.yaml): contains parameters for NMEA NavSat driver node.
 - [`relative_localization.yaml`](./config/relative_localization.yaml): configures data fusion for `ekf_filter` and `navsat_transform` nodes, using **wheel encoders**, **IMU**. The initial orientation is always 0 in relative mode.
 - [`relative_localization_with_gps.yaml`](./config/relative_localization_with_gps.yaml): configures data fusion for `ekf_filter` and `navsat_transform` nodes, using **wheel encoders**, **IMU**, and **GPS**. The initial orientation is always 0 in relative mode.
diff --git a/panther_localization/config/nmea_navsat_params.yaml b/panther_localization/config/nmea_navsat.yaml
similarity index 100%
rename from panther_localization/config/nmea_navsat_params.yaml
rename to panther_localization/config/nmea_navsat.yaml
diff --git a/panther_localization/launch/localization.launch.py b/panther_localization/launch/localization.launch.py
index 2645a6b4..5647c4e0 100644
--- a/panther_localization/launch/localization.launch.py
+++ b/panther_localization/launch/localization.launch.py
@@ -29,22 +29,6 @@
 
 
 def generate_launch_description():
-    fuse_gps = LaunchConfiguration("fuse_gps")
-    declare_fuse_gps_arg = DeclareLaunchArgument(
-        "fuse_gps",
-        default_value="False",
-        description="Include GPS for data fusion",
-        choices=["True", "true", "False", "false"],
-    )
-
-    launch_nmea_gps = LaunchConfiguration("launch_nmea_gps")
-    declare_launch_nmea_gps_arg = DeclareLaunchArgument(
-        "launch_nmea_gps",
-        default_value="False",
-        description="Launch NMEA navsat gps driver",
-        choices=["True", "true", "False", "false"],
-    )
-
     localization_mode = LaunchConfiguration("localization_mode")
     declare_localization_mode_arg = DeclareLaunchArgument(
         "localization_mode",
@@ -64,6 +48,17 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
+    use_gps = LaunchConfiguration("use_gps")
+    declare_use_gps_arg = DeclareLaunchArgument(
+        "use_gps",
+        default_value="False",
+        description=(
+            "Launch NMEA node navsat gps driver and include GPS for data fusion. "
+            "Advisable when the robot is equipped with the [ANT02]"
+        ),
+        choices=["True", "true", "False", "false"],
+    )
+
     use_sim = LaunchConfiguration("use_sim")
     declare_use_sim_arg = DeclareLaunchArgument(
         "use_sim",
@@ -73,7 +68,7 @@ def generate_launch_description():
     )
 
     mode_prefix = PythonExpression(["'", localization_mode, "_'"])
-    gps_postfix = PythonExpression(["'_with_gps' if ", fuse_gps, " else ''"])
+    gps_postfix = PythonExpression(["'_with_gps' if ", use_gps, " else ''"])
     localization_config_filename = PythonExpression(
         ["'", mode_prefix, "localization", gps_postfix, ".yaml'"]
     )
@@ -108,7 +103,7 @@ def generate_launch_description():
             )
         ),
         launch_arguments={"namespace": namespace}.items(),
-        condition=IfCondition(launch_nmea_gps),
+        condition=IfCondition(use_gps),
     )
 
     navsat_transform_node = Node(
@@ -122,15 +117,14 @@ def generate_launch_description():
             ("gps/fix", "gps/fix"),
             ("odometry/gps", "_odometry/gps"),
         ],
-        condition=IfCondition(fuse_gps),
+        condition=IfCondition(use_gps),
     )
 
     actions = [
-        declare_fuse_gps_arg,
-        declare_launch_nmea_gps_arg,
         declare_localization_mode_arg,
-        declare_localization_config_path_arg,  # localization_config_path use fuse_gps and localization_mode
+        declare_localization_config_path_arg,  # localization_config_path use use_gps and localization_mode
         declare_namespace_arg,
+        declare_use_gps_arg,
         declare_use_sim_arg,
         SetParameter(name="use_sim_time", value=use_sim),
         ekf_filter_node,
diff --git a/panther_localization/launch/nmea_navsat.launch.py b/panther_localization/launch/nmea_navsat.launch.py
index 585438f2..b121eb9d 100644
--- a/panther_localization/launch/nmea_navsat.launch.py
+++ b/panther_localization/launch/nmea_navsat.launch.py
@@ -26,18 +26,12 @@
 
 
 def generate_launch_description():
-    device_namespace = LaunchConfiguration("device_namespace")
-    declare_device_namespace_arg = DeclareLaunchArgument(
-        "device_namespace",
-        default_value="gps",
-        description="Namespace for the device, utilized in TF frames and preceding device topics. This aids in differentiating between multiple cameras on the same robot.",
-    )
 
-    params_file = LaunchConfiguration("params_file")
-    declare_params_file_arg = DeclareLaunchArgument(
-        "params_file",
+    nmea_params_path = LaunchConfiguration("nmea_params_path")
+    declare_nmea_params_path_arg = DeclareLaunchArgument(
+        "nmea_params_path",
         default_value=PathJoinSubstitution(
-            [FindPackageShare("panther_localization"), "config", "nmea_navsat_params.yaml"]
+            [FindPackageShare("panther_localization"), "config", "nmea_navsat.yaml"]
         ),
         description="Path to the parameter file for the nmea_socket_driver node.",
     )
@@ -49,6 +43,7 @@ def generate_launch_description():
         description="Namespace to all launched nodes and use namespace as tf_prefix. This aids in differentiating between multiple robots with the same devices.",
     )
 
+    device_namespace = "gps"
     nmea_driver_node = Node(
         package="nmea_navsat_driver",
         executable="nmea_socket_driver",
@@ -59,7 +54,7 @@ def generate_launch_description():
                 "frame_id": device_namespace,
                 "tf_prefix": namespace,
             },
-            params_file,
+            nmea_params_path,
         ],
         remappings=[
             ("fix", [device_namespace, "/fix"]),
@@ -71,9 +66,8 @@ def generate_launch_description():
 
     return LaunchDescription(
         [
-            declare_params_file_arg,
+            declare_nmea_params_path_arg,
             declare_robot_namespace_arg,
-            declare_device_namespace_arg,
             nmea_driver_node,
         ]
     )

From 1abf58af546aa6da8e325c1c815ea47da0ba0856 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Fri, 4 Oct 2024 11:00:28 +0200
Subject: [PATCH 067/100] Add Lynx light animations (#418)

* Fast light configuration

* Add lynx lights launch, fixed tests, unify arguments and config names

* Add animation and fix some typos

* Update README.md

* Horizontal flip fix

* Error increase frequency

* Fix config path

* Fix order
---
 README.md                                     |   4 +-
 panther_bringup/launch/bringup.launch.py      |   5 +-
 .../launch/controller.launch.py               |   4 +-
 panther_lights/.docs/CHARGING_BATTERY.webp    | Bin 51158 -> 27800 bytes
 panther_lights/.docs/ERROR.webp               | Bin 37586 -> 19200 bytes
 panther_lights/.docs/GOAL_ACHIEVED.webp       | Bin 37388 -> 19664 bytes
 panther_lights/CMakeLists.txt                 |   3 +-
 panther_lights/CONFIGURATION.md               |  17 ++-
 panther_lights/README.md                      |   5 +-
 .../animations/lynx/battery_critical.png      | Bin 0 -> 7229 bytes
 .../animations/lynx/battery_low.png           | Bin 0 -> 7614 bytes
 .../animations/{ => lynx}/strip01_purple.png  | Bin
 .../animations/{ => lynx}/strip01_red.png     | Bin
 .../animations/lynx/triangle01_blue.png       | Bin 0 -> 6497 bytes
 .../animations/lynx/triangle01_cyan.png       | Bin 0 -> 7481 bytes
 .../animations/lynx/triangle01_green.png      | Bin 0 -> 7309 bytes
 .../animations/lynx/triangle01_orange.png     | Bin 0 -> 7384 bytes
 .../animations/lynx/triangle01_purple.png     | Bin 0 -> 7063 bytes
 .../animations/lynx/triangle01_red.png        | Bin 0 -> 7144 bytes
 .../animations/lynx/triangle01_yellow.png     | Bin 0 -> 7601 bytes
 .../{ => panther}/battery_critical.png        | Bin
 .../animations/{ => panther}/battery_low.png  | Bin
 .../animations/panther/strip01_purple.png     | Bin 0 -> 964 bytes
 .../animations/panther/strip01_red.png        | Bin 0 -> 900 bytes
 .../{ => panther}/triangle01_blue.png         | Bin
 .../{ => panther}/triangle01_cyan.png         | Bin
 .../{ => panther}/triangle01_green.png        | Bin
 .../{ => panther}/triangle01_orange.png       | Bin
 .../{ => panther}/triangle01_purple.png       | Bin
 .../{ => panther}/triangle01_red.png          | Bin
 .../{ => panther}/triangle01_yellow.png       | Bin
 panther_lights/config/lynx_animations.yaml    | 126 ++++++++++++++++++
 panther_lights/config/lynx_driver.yaml        |   3 +
 ...ed_config.yaml => panther_animations.yaml} |  18 +--
 panther_lights/config/panther_driver.yaml     |   3 +
 .../panther_lights/lights_driver_node.hpp     |   3 +-
 panther_lights/launch/lights.launch.py        |  24 +++-
 panther_lights/src/lights_driver_node.cpp     |  14 +-
 panther_lights/test/files/animation.png       | Bin 0 -> 900 bytes
 .../test/integration/panther_lights.test.py   |   5 +-
 .../unit/animation/test_image_animation.cpp   |  10 +-
 .../led_components/test_led_animation.cpp     |   2 +-
 .../test_led_animations_queue.cpp             |   2 +-
 .../unit/led_components/test_led_segment.cpp  |   8 +-
 .../led_components/test_segment_converter.cpp |   4 +-
 .../test/unit/test_lights_controller_node.cpp |  16 +--
 .../test/unit/test_lights_driver_node.cpp     |  45 ++++---
 47 files changed, 243 insertions(+), 78 deletions(-)
 create mode 100644 panther_lights/animations/lynx/battery_critical.png
 create mode 100644 panther_lights/animations/lynx/battery_low.png
 rename panther_lights/animations/{ => lynx}/strip01_purple.png (100%)
 rename panther_lights/animations/{ => lynx}/strip01_red.png (100%)
 create mode 100644 panther_lights/animations/lynx/triangle01_blue.png
 create mode 100644 panther_lights/animations/lynx/triangle01_cyan.png
 create mode 100644 panther_lights/animations/lynx/triangle01_green.png
 create mode 100644 panther_lights/animations/lynx/triangle01_orange.png
 create mode 100644 panther_lights/animations/lynx/triangle01_purple.png
 create mode 100644 panther_lights/animations/lynx/triangle01_red.png
 create mode 100644 panther_lights/animations/lynx/triangle01_yellow.png
 rename panther_lights/animations/{ => panther}/battery_critical.png (100%)
 rename panther_lights/animations/{ => panther}/battery_low.png (100%)
 create mode 100644 panther_lights/animations/panther/strip01_purple.png
 create mode 100644 panther_lights/animations/panther/strip01_red.png
 rename panther_lights/animations/{ => panther}/triangle01_blue.png (100%)
 rename panther_lights/animations/{ => panther}/triangle01_cyan.png (100%)
 rename panther_lights/animations/{ => panther}/triangle01_green.png (100%)
 rename panther_lights/animations/{ => panther}/triangle01_orange.png (100%)
 rename panther_lights/animations/{ => panther}/triangle01_purple.png (100%)
 rename panther_lights/animations/{ => panther}/triangle01_red.png (100%)
 rename panther_lights/animations/{ => panther}/triangle01_yellow.png (100%)
 create mode 100644 panther_lights/config/lynx_animations.yaml
 create mode 100644 panther_lights/config/lynx_driver.yaml
 rename panther_lights/config/{led_config.yaml => panther_animations.yaml} (76%)
 create mode 100644 panther_lights/config/panther_driver.yaml
 create mode 100644 panther_lights/test/files/animation.png

diff --git a/README.md b/README.md
index e887b3cb..6cd6fd36 100644
--- a/README.md
+++ b/README.md
@@ -87,7 +87,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | --- | ---------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
 | 🖥️   | `add_wheel_joints`           | Flag enabling joint_state_publisher to publish information about the wheel position. Should be false when there is a controller that sends this information. <br/> ***bool:*** `False`                                                                                                                                              |
 | 🖥️   | `add_world_transform`        | Adds a world frame that connects the tf trees of individual robots (useful when running multiple robots). <br/> ***bool:*** `False`                                                                                                                                                                                                                                              |
-| 🤖🖥️  | `animations_config_path`    | Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. <br/> ***string:*** [`led_config.yaml`](./panther_lights/config/led_config.yaml)                                                                                                                        |
+| 🤖🖥️  | `animations_config_path`    | Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. <br/> ***string:*** [`{robot_model}_animations.yaml`](./panther_lights/config)                                                             |
 | 🖥️   | `battery_config_path`        | Path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only. <br/> ***string:*** `None`                                                                                                                                                                                                           |
 | 🤖🖥️  | `components_config_path`     | Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options). <br/> ***string:*** [`components.yaml`](./panther_description/config/components.yaml)                                          |
 | 🤖🖥️  | `controller_config_path`     | Path to controller configuration file. A path to custom configuration can be specified here. <br/> ***string:*** [`{wheel_type}_controller.yaml`](./panther_controller/config/)                                                                                                                                                                              |
@@ -106,7 +106,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | 🤖🖥️  | `publish_robot_state`        | Whether to publish the default Panther robot description. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                |
 | 🖥️   | `robot_model`                 | Specify robot model type. <br/> ***string:*** `env(ROBOT_MODEL)` if not specified `panther` (choices: `lynx`, `panther`)                                                                                                                                                                                                                  |
 | 🤖🖥️  | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`PantherSafetyBT.btproj`](./panther_manager/behavior_trees/PantherSafetyBT.btproj)                                                                                                                                                                   |
-| 🤖🖥️  | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts_config.yaml`](./panther_manager/config/shutdown_hosts_config.yaml)                                                                                                                                                                                                               |
+| 🤖🖥️  | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts.yaml`](./panther_manager/config/shutdown_hosts.yaml)                                                                                                                                                                                                               |
 | 🤖🖥️  | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                                   |
 | 🤖🖥️  | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                                             |
 | 🤖🖥️  | `user_led_animations_path`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                                                             |
diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py
index 37886eb7..c291d92c 100644
--- a/panther_bringup/launch/bringup.launch.py
+++ b/panther_bringup/launch/bringup.launch.py
@@ -53,8 +53,9 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
-    robot_model = os.environ.get("ROBOT_MODEL", default="PTH")
-    robot_model = "lynx" if robot_model == "LNX" else "panther"
+    robot_model_dict = {"LNX": "lynx", "PTH": "panther"}
+    robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
+    robot_model = robot_model_dict[robot_model_env]
     robot_serial_no = EnvironmentVariable(name="ROBOT_SERIAL_NO", default_value="----")
     robot_version = EnvironmentVariable(name="ROBOT_VERSION", default_value="1.0")
     welcome_info = welcome_msg(robot_model, robot_serial_no, robot_version)
diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py
index 9dc30b09..adf83965 100644
--- a/panther_controller/launch/controller.launch.py
+++ b/panther_controller/launch/controller.launch.py
@@ -286,8 +286,8 @@ def generate_launch_description():
 
     actions = [
         declare_battery_config_path_arg,
-        declare_robot_model_arg,  # robot_model must be before wheel_type
-        declare_wheel_type_arg,  # wheel_type must be before controller_config_path
+        declare_robot_model_arg,  # robot_model is used by wheel_type
+        declare_wheel_type_arg,  # wheel_type is used by controller_config_path
         declare_components_config_path_arg,
         declare_controller_config_path_arg,
         declare_namespace_arg,
diff --git a/panther_lights/.docs/CHARGING_BATTERY.webp b/panther_lights/.docs/CHARGING_BATTERY.webp
index 8a5e8238d1040150d8466748126fe2daeb5a5c15..2711b8fb734e9c67eff675b587dffa8efc4401b0 100644
GIT binary patch
literal 27800
zcmdU22V7Lg)}IA*QE4i%1!e8MM6pDItk_UN(PwX<v0w|tAXr}91r`0GQ8Y%4nh5ri
zSW!@8Sld%$L{t>J5DWMzpaO!hd*{uW%Wm$4d+*{!ls7-J+#N9U-*f)w%sFQYgZuXG
z9Weu;;k`V3hWT_EQ5hjrjr_OH5>-d2`+&aQ70EkH8ocxFJ%_ww8~YpW5walvPUS~B
z11q7Q6)o0VI-+i?JfplDZ)?~<<%tedt2FhilwifD>gsp0hn}_xtFy;D_)Z&#i4Hdg
zPd|D!aG@%E&ktjdE?ZRFs^gFi>Ct0U7wYfXmNauqN=3CEAqH=VQ8kVv|9d+Azu|#4
z)A$I+pGf)=t^XO{<0r2$E2L1ZU7JzUuWGcyZF2PI_d4OppPp-8J3b|*pxuM!p79~J
zaks-?_3Qmj>`dmb&fg90@bj5~H{*XFylPX%rwby<&?~nb5AeOQ?ONlV$8|OrU|5iW
z79MEhWAG<^(IOapk;S~r{z3z34%_~{W_fDhUBmhS$8A$w!ymplGiQ54g4UoW5VSoE
z%5#_S%|B+Ozv+5Fg%0j=fT00LIK==jd>mhqp7)Mp+6}yC#rdZ0OrrkR+yy_aN*htv
z0=Xsj)@jLTfMx~x&Ne=pVA7c_CY5yvReIhrsze$w2`UFzG0ac1JNH!^j89egrv00s
z&yTfmy*K1wM8XZEs`$rKA3y@AzGESgR}(7A*IA_J0FSR;+xdL0cMqYaN7D8R@Upw@
z&?YBl*0gSKJ#BZ#yZ*7=BYF(&yYFtV(>9@bfUSEHe#tN$`(UQ?%ARgd{Cj5{uv{Ba
zh-hhXM3ljSq~DQ)h|l139kRF<JLBdO4nNGY`LvFJX_JTz36nOt+{JjUCN=lmgNP%@
z!m6eYV1R^{!vk$52~j*jq)$q3m8;mU217Ba#l+wAUtATFu5pct{hg-~1WwZ>SpU_f
zK|`D|GjB>x%q#3$CGCkdQvDv8+3~8Hj9D5?GPzW5HqO7{u~X}YvByBB$(6i~-CVD<
zbv;BFZa*4CV-C|05xvbO{8nyJjQ!I|NKQsO7tkZ|$jtK#KjHj4Zywa{j<L@aW|VV&
zwEMVKk0R9-?y1BvQFepw%8cEWPhxy<O6$-$80ROuztw57w9GIvAZZ{c`TUmWAQhW6
z6wZeIm^t;3Sr}(fYId@4%;FK#n_ty*-2yVR$wI`uxsUPb-_AH(9**&(FQ1LbALc$w
zOA!h2Xf62(%wV817ZD!$u2T@lB9Gln#tDoUj}EU-1XR%9eejjU!+PIib5O-Si%giL
zbXFc@WQ;P(l-CkO64D#NWYU|IpOmLH3|6K%o_FLD#z_hsZ;3Z4U*F2?qO}V}ww1JG
zY;%~z^j6kBNh&IF6yZ16n@nrN#CaR0Mg2=x594Q@G-q$)>Qi(?JfxYEkd)Rw@a*;u
zUb}VcOXI%dgdFDM>5~wXNQ40(rw^?)(GZDht$fCkQZkAbi*V)A+HAb7r5uMBe%$t<
zvk(Csl+A#HpAi;~uw`e2#nLihFhulh<I7_d=}hEmXM?vz=`%xPQVJuJNTIOtN1jZ2
zdA#6^ER_#-7Yd$9f~Zd*cD&TjpgeoyZqvCHGO|^u7fp1`+6?dWb24LsBJ9jWUeaj{
zMq{f8#`Yv;C5mORk)9bVG#yj-UW~n}448h+W&Z7UR*J?sHzOiJARwDCc%Y3hn`xvk
zo2wNrD*0KVSEwdwPV`n^{b<MVqxK|zuT~2N2P6V{j>+pv@pSfEl9~OIv}IpODi)^9
zP?=1G)XIPhA}@8B8iOx3yzy7lgytK3w2B|UumVUFlT_R#DV>ZuherwK6tB;X3=%9R
zfYScSb8>>yC(rQ;t9Ls)$QrqLlmw5=xx3mb;bP06Kmv%9O5~}IZoO`&4VbNUiiZM*
zSx&o%#1dv=<;xY;2^W5{)QDl8Ucq=-C)@FdGjZPi*fZZwTT;)Z*Kr3lYeA+XfTE}b
zY6>(G1P>$=Rnh8`DXE592NS&+W7V<ovo9oJ{hFk_yDOhfk6(nUJc-a2B7)ii+e}tQ
zxee7isXUNy+gU4<-HcZzW(M!C@cmhLtUsK&^6}qy_vNaUYHih&1Aql=ZZ$m6X2L}!
zHIMWS;p&g;8ELL#Q!q>0U6?t%GM!0KhV`CAREDoL_-)wm$Dbat$8R?@`F!QAYLzSp
zJiEQ6SA!^2rGZ8ZU;vqv4#kiaW`&kE_1O|3XY3-CJEIdV`~b6ch{@`M@%koXYmWUD
z<E-n=!Y-%G-maT^&BBC;&Bg#9Usj|PAS$b1Hcyp>C>`U=p<|NV`xA*h+mNNq8v$dp
z(vk8@Jcbw3n;lc+vrE-jsQ{O1k)b2Tt`*Oo8235G2|pYN)cpQDK2U`kAZ;O@lJlEi
zbuJCR-+mj1<4+xIlN9TWvm5EFEvV$K%^HF1?lg1)F;NyBqEGPhkIuS#`FPeOJqr7F
zxN21ouO4Z>qbg?g%+^@w+mG&7+r<B94*$E7*{`}9vE^#kf!S?WZu{-=h=UW(EvwOr
z_pNEK?0TzXRu%t2s}Y-s3EbH|5HrEH{}+5xhjCO>zhwOFuEoQrdZ0$f9t3I$Fp9i9
zu}vzjE)!eKYl-nw-M;CYHE~#<{io{iJP!BEug@yE74ReN(pW)Byu6ez<#`KUj8}?^
zmf87GAl5V0YRUu=FK&p~wV>YPkbjfNs%5y&WN}hHRWjC-w?0x>KRWu8hOCerski<c
zoY&O~+1^qcJ%E(el>r_o)6AS7Q27<k`=)Dpi{T7G)2JPQq5eofV;?;_FF(19R6Qh7
zEQ>1h+^&4&y%2S??VmXq>gi>l_X}7M<t(HxKAhRT=vw&fmvR>tS8mxqyjIe~HFl@d
zh<EW$kDgGOesqXv1<+8R2WZNSBTee>%3s>yL2lino@%S}PY!hfIDkfq8)9hWuhVyH
zj9=9pILE4kVU%|RHDF*-Ncr4oal4VV?1LK)#rlAp>5nfh#;!9RnQ*%Wxknd9sue3f
z*`_K1o@_BZ($$cQu8m=@K+&kX3W)?UtZ0xmgrl$Ga-#e_^*yU~2t`#k{`^Qw;7~0I
z_!Y`vShJ7dF}PSnK}uUOt>?8!sc0&B6k7~0TA2>ZCZL$Jk4Q3me)TWm(f#qcgPzll
zG~e2DbQH48dTm4{6`MM8P<e2&k5a547+yUjiEP|vcU4{i&Z$<Zq4NNw)fP%f%Bpx8
z<6!Y68nbC9wy{JkYfK`-nS^mCWqRt}D-+|Gz8*{P+>o9s^y|ndqhzG`EQLni8kGJA
zlB#C8-*=BU%)|L7s;6|H`^NEdGqs|t5s#E~c$(rwex=Y9)032EdOI_<G<oBhb{f2-
z!@g^$)c}UFc@2Gn7t>5Y*$jJz%BI4{98+Taua(RI_qSb`KOW-P4&C<M>M+@<dhew%
z9;kB7UCDq3{GmYXPQCLS@^iTn=_p2cM-gmYVv;*!y!-sq;aM#Wd*g8Og0WpDGzp3Q
ziNvGcF4ybOX#;RcF=`0c){1CQ{_1!(DLmXHu-^WNYuRIcj|Lvztw2hm9*N2cJd(7W
z#8>RZ_E~_BWN?-S@9$%TlNUbE&1=rtxpPouyJ`7KZ6PYD==3E=N}f@CKN+m3cTbrt
zcOMZWDys6#C@H4%d=!UK;4$U-sFGXYNA^|mH_tc>By=LpoZazw&Y4$O&na)g27LM3
zx0^FIr!F|D!FE%|Ztok7lvZgzg?I#Vc<MVLy5{@NVih)o&eRdvz$q3RoQgF+*OjLz
zKtMXwD}i*RI9;V*ipV;Rq)(C#k<9W(3Xz1Bl`4{ynn$9=B3rp@<0Xi+%SZkpz#EfQ
z`AEzgBtvZGGLN^j)D%oty(1EH?<Lcjaa~q4FVhmnvdt7DQ6~G7PM)4Pen)DpT5|8Q
zNV*FC@8f0d+QQGzi4uh+z@4AuZoUoiW^!9QI&G?MSBYjhHkWq6@E`$L17jOs6B9^h
zqKL(mK0|JQ<^mlQ5D3hOl%2)v7yPJ(uv6wHB&WTk2$+|eHP+kF29u>i#R+REG*mBu
zhR004US#F!>^g6c{C3Q+Lo+7SnYMLmDp?>=-eyXKhNm6AorrDGmflUl*k@t=c1=zJ
z6Gv9`emH4~qN9-Gb_#}aML<)+vf-eajV$BDk2eTsp$$}sMCYzbJkLG*{2@5`&}^IT
z8v@2=KnuD_fTEF_nR&9|Xi7G7nf4DM=ff#WFirXhrbet_C>E*6+>D2nx4ECW>k26!
ze%=x@lQc&{UQR#R)a}qBWS?o;MIjXdJ94}}cZdIMq)5{apY=}L`^jMyvOAqgMbQ|K
zTZyx-k}dV_S8@$kuRJ~2wqWwOK$W6fUz>5`KwMz&A5?F+K61xzLNs_lCVqCGspy_{
zWX89U#hK6Z-nX^mR$&;Y`)in+OW>QAeb&*d1JnFcGw1u0MTlA{qfDAxO7X!9-zNu>
zp7)OG3$m|2W(qC{){t+8yPEs0>iZw;kQIs)TL1xQY&AU4X2L@~G|U|l93~abJ41X>
zy`>dG`n}UOQF8363u1CnyEuGLt1(<q!naB|W35B<m$@U5)4FyA5n2FaoI5Px4O&G{
zinS+CP<01y&2oG-JqO0+cquABcyx7)Gk%F<zt+3!)BJnD;G_W>@B$X}-48r8L8OmG
zv!)oDQdpdzj1K(Uv8w}md_YUa1We_PO9EbratSI>W#TD|m|g3KV0O>FQf*IKquPq;
zMmd!yuOuKN%4(3&$MoDEl1pYLILhr0sxvbYz#*a=wwZh+)!7Ep$<rApHVU>G(hAiz
z6g%|EMn!3TZvSQ{v~P)Q5lqK#Z@FVHjp4Hw>yCyLbje)r{$;gcRacK`*eJGl-@xUo
zQ))Lp-M;^#iR-K{w&8WRt>YG12$XyS4^i^$ut?2CBL0@l4&Ps#$%*XbW{2iuaIHyP
ze9iikeu>#wECv_dO?X;I5@(Ur1oZUrGqn#Y35lE){2<#*8liKDAL$I_#Up&izOU2Y
zFrTx{G(NTUF{G21d2nnUGLMlP5`+#|7hN>v90&I&l=wci>qspf4p<=N^CZ|PD?i1P
z=F-#Fu3PK+)op)$&$UIU$=}v$6BZT~yyeTmjAh8fLQW19L}l*%bM2eg->*tgp=Ycl
zN+FZOVkf>`v-Y<6ROpAEL>)Cw7>frFUVFyh?dPDlALFc3gB0G)n-;5P$jk))){xEO
zJqqUHiZLDY?UyO*dl5a^>`qn{BKnYwVMta=7P0g0GcRIW=Qe#dG|K{2xfTgf%+A<6
z_Hr!87s=XK%%nWT`diLKE@5uZzO}70FEog(g`V5sV8jvz547>wJB{>Zb1PpQEMUbM
z&Z1W(h*+|kq{oIq80+;{a460lxX}HX-6yadOd4>e#C4VTG2g8W!OVs}__`;~pX&Ts
zK<2Eg9)CVZisndLh(;j#EZYd3c}$K5?>>)ne#E$a+?^8z_o97<`XvWhqh<{Y)Gv)v
z62N7OiW9l|;!8>1LKUt!9g_uF{5B0^H|Nv>Jhbi#*Inc7{tDUI(HeO*(KQ2DRC^y5
zSIvtU-@1$qwd3&C&TkO-=emh)v(|oZyWRpdYU6Kb!X*&bQF%eln`di$dCBw6P-eM(
z9~)fb@XnCDS^&#w_p65Fn6UMEDj?vwfTT*wtd^bM@#K>=sW&mW$$iX)MXKPRjGHoP
zofh4kchtehsKK68X7FN9zl>D`*buNr0k^q0AP4g7(m2v0=s2=mPtH;FU&r8~SOZ_o
z@-I5=xkx5Gk<VC$EG;=31Y}=^xJqt4QnQnoRm+a0sHaJMg!4CPNbxkIKkge88x49d
zkA7HSR-o1bwJmc|ZoNmMcf*)g@c0)WEmcd;QBr#1c@xfTSynT5iLtx~&-UITn)iXF
zXsB1$UyXK8^Dy#Prd-M-4i>;d**k-PEt@)@aDo6nSpW<5eSn3>-S?m)b2-z;%pGCo
zXB$7_9Ys2MzLsOdvRBMZ7p8#*1TuE20l%Unb2K2^%ahB>n1z2o$>(IvQohs-8)Zfw
z<YKTryPxO$mhD_4qwf3!t)+FM704c>A(eUX^iV_^-NzpiJ9R2@^3#vRK;-zhAtCq1
zlY!&$sYAWmwh7mI6$~jvvJ@cE#%Dwz>B|<2sb%kyV{_;E0XC7C9FLD|6Ay=z*6eOY
z1!Gn+yecYBGO}-NVg7$Nxg+Vw$zOCiH*l`nVl#YhR!6Xw!#2KTz<#A9Q~9rVyBzII
z04e7(%+l?c>AteDo2~ykoUnCLbsw9jUmqaS`D)ZFr3K&s-Uw)m`cy!DK(g*7{}wqz
zOL92zl>0j0HpfoV>&Y~p0EglcuqtZR|E#{Tw=fplAuqlq;N_1Z)o?9h637PmVEY#j
zAFOo3__zHtQ<n5F9@7WuNaZ^`jd8pi?D&+YF~bRRV42;?nD2{UjJ)JGrbF#rYAbtt
zddxyhUsOJzud-ls=n;%}tipBr=pnPm%qPF*9PhM-s*B=T%|4P>6Q?PEpt8*8Pm$VG
zY0l?g*Z><9{<G?lXTCRkA=!<X?;R8gDvM!yibsUmCP@w9FuUkFF4<ov^LDhf%NQp<
zf(_TqTW72vdEsm@-Ju|AX=RKj?>uimdf2sNU%fy|2O=SmkCZnGG9#Z-6hNL|)n{vH
z`2$~Q-EwWF!UrwhxVR9Ll)aUW&Wgxr<c0V*FHps%r?rKM1pH$eYfYL(|C{#qF+7tL
zmp+c~E?;w-0JA^p%rE7Sc>3bRY4YdVl0`>yF^TR)m%gJ1LdV6^=evbI-;4Kao971C
zzn|0rju1L@vL^n>{5&EhCs9n2H}sf~ueB5zyqF>Y>LL9}PohY?FFyn1XC57Iv4g{W
zCSYBZ&k#9&_<ZbRXQ%%Ic{wL$md^*cFmG%}q&P;206ZvM1S9YHe4I#n-aj8hyz#c<
zW5V&Zv27+$2sE~mst$?8F{Qe_H23RvanFArHE6#fWSLiuOF9&ptR`?k9#RoQAScc%
zy3?{Wx5Funx5M4s(Ysq{a>Ie`XP<KMUlycNM7yleft*AvmU<4bSSe2?@9Yt-MV7e8
zJsqS{K#z_u+fO#eHycA8E1;3mnRNUjTI?)x@xEzix}IEj$mH+DctZSW%Z|QS-*WWr
z7xsgwV5nr4f{1A2a~QHb7mm)jxr>?G;Uv90bFwBmx*ol6w2tI0VL1UCr##hZE&da!
zCgXu@V8<ETOn9ho4I`Zi96SOXlD>uQ+%bB&e9;92_B=gZ_$Sw}T}>tBf|jps<7+Te
zS`6&)xC1?dytvQa-rk9T3J^-o)I5oCypB1EjqWV8VT1fd1+s-kKTw08R$A99sdF68
zIxuQSR$^9EdYT%wzMZKyVk)r~IhbbCJK|*BH3sJ!D(nexTaEFRzcTLmUOBpI$0=(x
z;j|IY3V5K+q(y3P#ZDk|@D$2O^j1cx(|+tJd)JtnIioSN+&$d+jD6<kVFed;+6p!|
zYP@qES_GBkElbjImLdxqYPGQpeV&93E`t_-_9pe$cq>%p0hJA~DWXtLoH>N@9cJ~H
z=J+nnc>do3F&!gSCKH!HE&*Vc*Hq~8PLV=Fa^GZkP0t_|(#!)<QI^B_Y-O`1g1=(Y
z9{PE=vW0ow*}-_H-gkm)*_<{i*uCp&@_R!pVb!@7g1=$nv8kVhR*T53Z0#!V+NDfZ
zCL^oMS8E~WQHUnre{1D0GrS3aQ&%>xaaj4v$|>jkj`uwNM&tNCHnW<s<M$ju>T$`9
zHB>XN7UG;P)#lhI$K9`$;!s70%v*U7M=#v!ATYQrxl%r@s=WCgsVIa$gYfrPn!Dj`
z*!39UT^wzZc_&S6YAxXw+xT9Q)B$DBwOzZif9Iy?<}G?JLrJGKimt?Z<YmQ@d@ofb
zP@bYBfsF4X#*b1A|G06sl+K25y(Wi4WtEL+3vG=vvzn>}(IV9Xx%==jyWjgeX7#hL
sLtno{wmwm67#?PL_Pidv<6Aux?0H!tC+d)&rLG#*HDGKj9Sn#53-*cMGynhq

literal 51158
zcmeHQ2|QHa7azMyDJ_bGvXrQVsAQ?^lvafhqLlcRY{^oh5|t#8B2kfKDSl;{l2nLR
zOR|+U%-9E+`+skkiHX5H+wWiR^XlEX=FR8b?>qP0bI$kNd$+16D!xrep|lj%YwpyP
z+BF%4LeWD1uF|1qqEPaiR5Tc&Pu++7CJjYe8sew!Z+xdk(L(p})Iil2C!+#sg#74c
zp_XZ^3eX7V<Kv@8sjI2k(43MMlS`!OpjlMIXs?@EmgSJFd5#N<^{(KabFjS8RTJax
zdUXXZR)4vRjB!KQJJH|H6<8F$ODc&jG!QLRy@;V5$B*S)FFu3aaEYnq<R{9{933Y)
zFy?sDE99NqT}b=DdxieZllnAqi{ximFZ8uRMch8mURiP#ZO)vtanBj}NAl!|?V#og
zBQcE(iCNGO!AP`1S054*GUno!RZh~QP<g(-^>mE3CN8#g&9q7%QOm4+j@`7Rqw#ir
zEoU6GQghy779U1cI{uyt;fQ6N62Wgv<2zRrEm3}JGc_gOtwB}sXu>WO3g1y`5rp$k
zVSM`D{;Z@7j^|4B`N-O(tOZ>3JaM)wbE|c2&hN9>yUQ`BikBhenQw~fw?w0*$0w(@
zy2~?*EMR%mp~dxWo=)CQ?zlC~^82y-qTVN9=Dzt7bLdI}Gc|DZ+ykS+JOC=#F+fGo
zqFU<YMM03pDv@;Rk(9_jy#=$(-Xuyd{&dCGc1wg178|m2M`@|z{fN}(p1ZZ&PjkdX
zu}+qt^fgrka6r9BLy>U+JnEqzjvGu$3ihK=Tjm@_8T=+5#0uIsF{QEE^XoaCVyBk*
zoUgR}(&c9{>$d6I)6OV-y9}lSGsov3z%z^7ZBF%6JMsN<oFi`b1(%sLpZturd?>kg
zDL~*+NE`aT1GC?>`>7mWH|aCa=SxI#Co>q8Y(yV3nfuWSCtz&yI&yy(b}2V4eF^n=
z4A5r?+5_%0np23{L&kiSzsl`J3xj!-QO;A(9ynaG!qja)yR)nu--$416a$-0m6y_i
zMRAu3l<WAcz1OO6-S&D<s<6bwi`Wa5GRjZgA@J0kop~j***%Fb`xE5tp`L>i!E`y$
zY|aOu{ASygDt9MTQ|nbre8T(X@g~bp-oO2#^q!xS?g92MvFy)wZZ)+0m?`@}+@mhv
zqa<R9sFaxrcP@uQYi$~KhmYTN2{G0mo(nsVSyGP)tYI}2Cl4SajnEH|wv4nk7ULf=
zr(VkfV4YaU7aWKdbUXO)<vQlBCudW|1CqTSu$11vZmxT9fve-!jivq?#Y$R?mY+AR
zdLVIOz3kyzmRSOrkcd-K2L(B`IMRc;<F`odi(Vz?Og$#!jYvy+G0@TblDb$H)Ube8
zVt^<6V?=tv#$cE0zTS8EZ{K@;K<J3J>To{zr@Jkt9uR_z&<`LZwb1W253Wa8(TFrj
zQ)4kmZf0zB0{CaeIX2B)_z-3Ti3e<Y(jAkQFE~Me^@7Yvwe*|v{NJ*Jyc;TWRBW57
z_!8}!`~8Av5JD^{jWMH-MMEP1rCKk)i&y|~!1Tw4jvU}u(6n0D|K&%u;6)F#t`6()
zQci@71L}{##+cE^16zLq;9{SK2Htc6g(b(O7QAI@*voXy;9Tl9jk8MMkL=xGTsO<A
zU~zTamJj<w!rm!q-0})g;97NVjrniYYd;txF+d1Cr61`aj4`kUa&d@b%uf`eQ3k7n
z0gY2WC7VBW0!3n7FP##y@v1NC%L5JAqid|16tw2nh6*L5?_09t<=Q2x4@C1-cl_h^
zW^2TG>#bLGjbePlzj675c7JKhsOnPjUU>9lr_#0Iw-xM1s5>y#vY_#bt?=OrCx9_X
zu4Vsv5}f^z?2#C>$=7-&O*uMHxK@BJ!)49xJawk4v{tbX{aBaLSDWr#u|Rjy`?t>x
zq!VU_2D+l{ECtopPTD)e7O{j-(xqd9$Vq+BL?87^(u^Cl)h#~}iit~kbE7M3^P&CW
z!8_K6pWZP!eD-#syE`-o`?$fGdd<;odyxBc@Hj>E=f(t!sm0w`TucGDnJKh3C7S^H
z3`X-b?}H}WtsEH1Ar|TkkRm}Y6+|T95q2z)sCUx`6=)ZKZgp;e+onJHV5x=^+GTb?
zM#>r0PZ2vKwI7)c>9XzlAs3`r^y9S<bSUh#<n+=R1KH`aPUY*uoDphK%tR0kczh+7
z45Ft$R+534K>C7`uQC?B<i9nVjDhtvQA~|Nqa9;_#RRiNS$_A~2wJom(xPKziB2eI
zWiVYj`Vum+{8&o5G;VO89+5{sF{2#giO9r^g!RKR{Ee9YjjEFjd|0RU6Y(JUaDOW2
zLf&V#L*wvYW)4r1nQrD*9f~``s8m4p;-t}rz}z~c=wHOeTh0|r6`z9U4q}rDG7Zg?
z5zfshXAUlAowRIkgGsXfVufiqTa}}%9ukeXnH8Hk%xC_7vO<*0#G!5BzS7SKL(r!%
z*D`$wo-N-AIoh*po0SG)pRB#5Y`*v=&9*$n;}(8-&tnhQdGr$7{p;d%d6Hn})-n6v
z#Q6{KERR3dWWT9iN#|T5H4V{!h}oxpe0_pkkTaAk`{S`kilJP1$!Naa4;`*N-61lO
z0(228UWVLxCbnpm#z7^F%P!oE6<oWmV#249#C+HVSUxDB@sZR^WXuOdL$`YI51A51
z5|K}2YI`bn@6=s&a6@3KMeE+gt(UI<B5of-($RLdxGQJhb*XirPL=0&%<W^Rwslp%
zl?_KfqQTEw8}^Y2XIvkk$z6K+iMgDU+otNls<q^Xjlr)Q#vP3o$zp&$oGfw#fWSyM
zp4->dKN?3QE?Jmn|5^L#b8lg-$bG~37&sFVwE&|*DGoIHzDsXcjx&C)MRN1?dDN0l
zO8;>_vNyq!A>%YPFjvR4s8Mac_yxhu;sbIi{SYo@VXt+Zoqq}auuHI9^kaV!#uV88
z8nb9=@9=TcS)+Sjt8A0NL+M+)q7BnL<_Hh*a!xmZ|1g~YMePCOfg|1AZD|nWr6o}P
zSi}f_w5EVcgVsHr`O87qBw6~KCpQ;f>Oh-z-mt42!l_Prd@2ga+K;7GgZVauj9cXl
zClJDbwp?cqz+I844k>e`n~p48{AQzhWqdiEhl)%tOVM#hO{uucb(UG=RR#e(a#XaN
zcnjQqIGd;Y===VQph}8*l2&#ET3^@&fX1x7>L*^OfST+uSN;5EhfeKs<$s=johEra
z_$9_l>TA6_XHw2Ln<w)Rw{7q&sv`dBhj>1JLQv1MQ*(h^|DAMW-_~bB^^{s~19<Di
znM#;SP^iTolBgAA34y)rh<&>)^CPGIN(nT_XG)&iDi-)0jl#!bK*x3dSE9`U06esk
z4aX$enKbz;MhU6OU5a?)ox^9=csp(qN8&x<pBwX$baX<z_ZHNq?t|1;g62C1RZgEe
zU_Mvr(P~R>XKrWI-571%lhfBT%QN%R^SDkQxVX;q(Uh|6dz0^!)t2{PDng^WG5|g%
z;`?1Z3jnRt{8m=B?+HkfV<tQ)EPLHY_mdLYy_ZnssK1q;y8+NUBQ2kz%?dh&aOxMj
zYU0HgZE^ejDDt2->A%GaB|JUPNX-S7Jva-u`{>)s_fRFJR$9}HgzSY!fzEknmiqtX
zbpmz5u1llZ+gBdqky6#8dA=1pjqR$lrB5QAYiP-Q{^h9BCALhH)=!FooKUI{xW(k}
zl{nV?f4$cmB#cP?jNqfWLG?Z?zxr9P91eDwt28(o9BQ%$|Lv6GPT+*;0x4~HFmN$p
z8%rRc_2FwRwMqS#u0yJgQXi}O{V56BAP1@+l<#nAT<}585#<P_pCj<E$4veG9p|XG
z^8P=p9O~QOJR*5j<89?q@`lI4%AX((&qT~Q99NeZ){Nv?WyDb#wofS8A%wXxh%*l1
zNJ-gS#YM4DQad41(qwDC@d?|Sa=T$3w!dt7($F@H0*u7C#uF!81Z`Z=_)kSw<AxGz
z9*Ma@6DIOb&dflp-5<`(;7Pis+v=YVIZJ;YV@FA6SoP{90+&@>pnwMXJj=z`+EB~p
z39?*vQ%U73^|pL1Wqf_*CQ=0yOW8e8ETwyzrATAQ()tfaO~u{}X%=PC^M_r*IqRn=
zof2`pl5YEQXX|YDx@jk;YfH~N*t8mPHa{h+t2Y|`@>3s?M3&&$ge{@}CTy<sthf@L
zLX&RI&Q04t4d*7oB{S*TyqJU9vJR8e(Py1&GDYuJW7S_}UDkfrS+Y}xUY%*@9^o{s
zGfBdSTNq$$C1?zox$MK`UK?mOAtElf^$j$2?s!zo0dZh;^Ili}F+*+vv*&X(UnkzX
z>~m9|FXq7+hiehcyEh0lhHct<g5LYh6W&IX`@H*D0^VF(t<ub<CRFAA+W+BcpOqFu
z#^2<I3qh~%!)TCe3!=>h^ZUI=lU^}`U-OX>^0-YjvY=GEDBHj;9F*nVUd81mbaYw!
z0`2qz`7S0NXur(`D_PUOw>ex8F^a22$10Sl3)anJ`qp?nS;;rLz}jH<Ev&%I*7K(t
z8cTY$YPdxh1xo#+xtF%+2Ze603{h4cmLHn<bEPw;`ErT{m_>W(MV(pP-di);OA~yY
z_sg+Y2Ckkt)MJLS2U>2p2>kj!j0M$kc)A`B6%iJ-pI{aSpVd{>kPT{TR{p&7eoO8Y
zMwTyoEFT(VJsPB#5%jAq!!N<DhqLp$kG?Upk5DD${y>ONAZmLVJH)`S0uH602e{c;
z!Q0QN)3+R2Ki!p=nc{zR&OJ{|%>`~V9Ovym`nLHAR4G1?^cNT6*ytdxa+X4X^V~j*
z5cJ|4yq#n2Cmj0hzSga{Hq|zC(dxogz=Shx$-K9v2B@F80NjtN&}v%R)hG+FviEmh
zvBJ$wInB_0(0@WUX$x){%%yf8q?QqMeI8V)IcVO_qAtX<QL>u={-wpcfMyeXjVx%G
zx=E_zY?isG*NxX4{7XK&vMTBvIB(Ue{BFsnI3D|zU(}86#3-IsLiap0RTsG3l%h<O
z^;u7p4UrsI+;;+xf?M{V(B{st0eDI4iaq!7?-!v37%s@0h-`CQE4vxxH)kD#-y;oN
zZI?8y!LH^xhMy$A#WrInxftc_J|#z0!+UOm+fQj!8;dAY?K}YL<<?zfHKxOD2Ef}N
za2`2QeL$&1RA>9-1vKXhf;6!#J3rC7I~zW}J4>2w_9MCAlDU!L46LX$bIrxJj7!bz
zPo=_RF|`Mb0;PD$SfEh9x){{oNV*es;79u@0DK+roNLa0OuB({p`~Y@&LcO(%d~}v
zrl!G2P#WLI0tt*IG-uN;#X_>K0^GC!?f&z=hh!-I6#QX8yG?k?#Ufr=9TeL&16{X<
zNga78pA6dXl4mx5M$?DwV&_d<@Aw$SzY_iL)Uuf^4`~D~sTB$6J{Sw~ypuSHJAwT}
zSXU0pxAd&0fpm^u9sq4OWq-jTu=+xt(n~|vupeymA{349P4)I!rK*{~WP@?&AVr!{
z^^N{C+4!q5;Li1T1R<gT_4A&iul)o^xQC!CP<%;<EvRudCSXY*&yFB^$gtH+(Ie>2
zOG0ExjlKB@xA_0pANUi!yFc5Xr)2q2_Jl?vnlZ5=JN;lmu>9a&F}OE80_&(vjDR0u
ze5YjTSoMl|jC@3V0tJ8xNc1BW1ilECiqS=4;&TIXG~L`8%nkI$;?H1>$oq&c-JiN@
zD+-c>?t^3p6Z#08yVzSUMqXRAzrf&DDTjSukQ!M}v&=uv?6K$0ci{yRI*JkdKbdnx
zhU^$dl{c{!)Ldc9kX#~&wlHiXk1h_swC1e2c_)72XpPGSb5w*QZc)LWy^?2lcBTe`
zo_k;<`tghi5&?Tgy^-jLF`|Xm@kNlU-w*-iDdpgcoSsEyIn*9Mgp0IR6E<<)5~BN7
zlup=pR~9Nn@!#u)jJt)S{b>|Ek4n8@iP>!=PV320@z0rg#xXDVGjK>`eq+6j0D-}M
z<hWjAXg?UhtQjH{y7TzJE7^9YV$9==jlLPhopf*bhdyTs{$}Xwn4Kj&2yht8Kcp-P
zP^=vh2H(~Lm@jI$?bIcH!Na}g{ef8-GP6)7ny&)g_OYDJ8=zo-w8fuEh>^4*!H!IZ
z>T`O%_kPy-GGGEYpozM*scXioepcb_S(cZpwLD5em**94`0ZTtTfY|NT`q?zlm#tS
z(5kcCpfcnqaQZ4(b39@;V>~y?KPWOCEpV5U^la1=;P=@`x{=g6LeDOM>PPo|mVbnK
z6VR#64PAc5JH7oB-D(h1c;qqPF{Y#nkJ-_S?ykd#?}|LjC6BSYm3BzEhV^*XoTY|J
zn#YUwN9(Ybke+<_g~4=MbFcS0Kz~M<4N#AQ*@Ber(@(Z-P2Q6Q=C0Z2_wK_Kr#N<X
zwhiG@H~fb8x&y|5(g-(*PNP*qhyZrK1soRdB%E~sKuLBqTWIt$EVpu&F1aVpqCEGa
zD377s3zxHWY>a%AKGM=5B991u0i_J8(MF>2hAhCe=(iEhs#LIuH-4?nzrlX{-c--Y
zauAB<o5HAd@`*yM{^a{Aw9l@TJh*fy_jv@H+{1mgOK<B)V8dTr6rQIynfPTC@)<*v
zR^=KK-??Q$F~FKSVasKqv*azPQJnk4p)tq;Es9aNc2~D6lU?>GLw4b_qirjO_F=g}
zFVP=%5$-GGxtBz15Z0ihEg`LsivcdO=WNgcEsI$8sI6<o=^RnHzMXB|U5D7ah2EyK
z=`WaZ3)e{UT}zW+RN$Vm#`CqnKbp#|!HsW2`uSqQuwm$R1B}RYGCzShUl8WwM;np0
zVts(CDDFNRvQl6~w%(9$`g+V<Sx`Xi+Tp9KzQ<@`VpHFgn$OFOR|`0@dudyKX}RU&
z^A$RwS^o-O>8!;zCg><06?tD#=&Rb*YHOz}60-VAj;*0{uXYZ%2;)M&woU?s6LD>w
z(Z&U5g8}XHox0CPDT1VE;Ue`K!@cB1LLVp=Z4Nr;GP}JZd^P=J-+wvnWKIgvYgPK+
z{mxdP{oVX~2lL$-`Zvr%E~*_kcq#sJ)xsbJN4sz@2_{5~ArLg|EHY_<`VYa5f_;lI
z(J2#DWd7<F@;(M5LeAS0wJprsk9HrLV1v(J`=pP6z|!&8ZiOL%k?1E@N6@NptZoRL
zwOscq)QDE{A9q&<#^KNPW8af{M_&&f&S%{|upf(Z0vB3PlNV;8S$b`$)hC<vwH&3D
zxJExVK`9r(YT%YZyNCT(V@;GYIR_c_{*Pjef1}O)&WR=H^)a8fGwuXLf1@06wbDP0
zks{X*^P$One`h-qeS<OJtuvraCH;(>t`%;^>b$%C;PAT(+Gz``^=EDiiQ=ALW#_ol
zf#cTuAjvu2|2|b*cW{<RsDs8PuSAEV!R76{v1T5s&V|~l6?Jo4?r&Xor9ooN3S^-=
zf(1y)7N$H(686|cz|G;BmUqu~4D)5kx~Kp0g;&?wgadk#*0s~tynSEKq2>AYDV<kr
z+*7wJ-)s(}zum7}RvK#i^~*!4A8XafzWCS*`AS>;AVJWXupKeI_xzX9nd?YcF5YqV
z&)jWxTMg@@q?J$X&XWFobhE_u4_T(-O~d%0M>6TCD8TxXTu%^t2t3bmbY%hyq5v*V
z_Vwmg3BRa+WtG^^XPmqrual+s<475&WhV0j-o*#B%-Fa-H)%a7OmHg{qg{GNm19SN
ztNXFUPxbN3c;i{n=gQdJj%Zxup`!7mTVNC@<pPenHMOG{RG`6^?4L8Ln5y=FsET=!
z5E4?VI9qy2^pO6Slq0oe_yxH2{mcd<j1_s%&ry~Eq}FqV5@BW^HWmC_f$z|-UgleE
zRQdIgkXz34yXGvEFM0|kJi~7<@ea7%<K++GU5devr2xOr31mjKy#gvPW*tx5?ICI+
zoaPrh$1rmjGw~^@xeo9~3Hl9wqkZ&S|2xA%`RRz}a({sqjWo3Z%_aaJSX^R+55E(I
z##IP@lce)&_<Bm+w*Fa|w2i@s1#2H3xWJVsp6*r4VU%t?6?OWgGc_&lzGuAqan05g
z@Ff;>J(Yzn_@tDU9qSxfb+WQUS1Kpn@K&g8_Fis!`_l1sd>;ALe0td`3tR0~*kj)l
z|F{tOtWHW9i_H|h;E~Abtv}4&o2Tx$=|qpu00yD&*4ZnVu`063Nk=^P`mFkNf1NSq
zLYTk^tRhTlTpL8ABj>BdqWbXFd&=G$L?3lMuMzp~S->={sA-Yc<M{2dHTfCqDSo(1
zeP@8~g!=;}i5P7Z8kjJk6XV`hvbhW6-kt_h&F%+@NuDmF3pJ^Zv}m+motUb4HtU0S
z@%P%Jc5a?C1HvAqyw0m=n#v(4sd3Qha&%S&hxn$${-1%MQNSzaU5gQ*0b@bVwGmk*
zc<dh&R*9Uvhs44{3fe#$qfqa^wmSCoPCgfdH&`v@#|KMyoaNcIGI92M5r5ncL*+Rv
zQ4CfdCdspRc{7|^`fSg_{3ybgX;!Hg(j!ELoIPQo>-H1Jf<0;aR19Y`;KWj(6WVQT
zYIw|f39%<%v$t)kym#m5#}EJ394KrNuebNNS*Nk~Mp)n=f6zmwLJ2))Jh}S`Cm?@y
zNUz=n>yW>r3XB4|&LC>iq0W>Z2_|Wx7#lDi<LNz)ztu@;?%?QpkK^KI5Om@n<vIQl
zv4DP<B!b5b&s!LZbqMP;L_Tx<xQ2lUQcY=equi5`#)l_TEO4kOjQpIY2@)12$U;96
za)KTppV8D$&O5?+JQIy8AkX(6!?}<D0S*a@Fp!v&i=-G6Vc)}GK0ZErl)9Rl4b3TO
zF}XyV4w^+ZjP|;zWmyi%n&-H%Snmq%IS0!dT{SW8u2)y!V)d81$QU<-y%YWIT!EcP
zTOSg72)SGknUgW#76mP;mtEyeIpV10#lm7CC?LG%isj?4U!Fd(fBDnKs>KsmU;Tr#
z;p>OblHZ=*xo?qfvw2ssSN)CX6t1u9j2zeg_O->Y5|PFcC`Fn^ckIB4V7g9}PfJ@2
zlBy4$mJ+K`@pIBW!2Ttc{n^f~hL#^QWgm!p)a84WL@W`NGBe@M<xps?O~dZ+@w+Y|
z#`?o^VW+hrPUwyLCXQ?FEiW&fc#_b|=^J(t_6I0oG1^f#c#-lsm36twPAx?uf(j}(
zln<$)i&z5o@}JnHy5jX?Hmy&tAw&EaUACN*i9Ku}pc`N;`pG&Zj3aP9#%N>FZB<Cm
zZrpZMA78cdEytwUi8S)N?mQR2L`!53uUH53ezlM|9+`2*Jaxsw1lQouJ$IFD`qGfw
zUm&8K2apMZ-h{oUi@h1={u!AO$-XSp2Tk-{j_pm~mxHp;@l#ZCcQU`@8{X8=OROxK
z)W`zy9vBN8@dK)F9%2bpPgy1)z7gSEvKsajkevyvz}l7{;3wmcvI3p2t%0}2I-9z5
z?QQQIV8Q<cS_^J0DcB&JBKs-*&N=Ig*_iB>f2zxfUs_6fdTI)A`(fGdKKiyl2dbpl
z{t;L?>C+>Czr%g`!t3;wRsc@xb6$*9Di%ohH<(nffiAI>nm>(+&ZSR*Gnx3b)Lh~A
zmXU7k+uk>Ww6|vuH$*$Q?SrV+cR!n*)^S&M0x-wMDmI3#@-xp{lTXu`#jZWtNXt@T
zz6Et^b-;GH+02MX!RFsvO6cW<Q2h{k`ADn_R&o-6*r(0)U29X&>ELIm-W1N8&WwA*
zri+?z?4_GCP{E*!^ypL+;J4h52TjoQum?RB>jhTl!Tf7SE&i!&)dc`nD1X($)OUd>
zd3D33UNjdmwCp$NmrFD2*-lTy^4gWGZ$h80beWP(szxEC9bpV8jdo*!L2Y|V@AXyv
z_#NdF%{~MO^5RtF3j*WT98%xwUdqp8W}Q<VE$YVY>ce&K%Z9KQFXuY4X<FYgpB6B4
z?%uYK$oH(IWE+nK4)vMPx}ux)iY&fl;XeY<WS}LHoiabOq<!Yn(1gAD3-?*Yv#jM)
zf8bl{ZhZF2`@$8qmsUQ}apUq9&{DHZjoJ97`tV$9<!gIHp0^^~=$K#wk#kBglL_Zy
z!ZQ)Z0u79hY@bzD^F($8VAjaqD%Po3b^o7;Z|s@oyS{0?T<%v8{W9vjzUU((PyM;c
zMrP?8$)XM~KY2+w7`A&x%s0cOHF55gdOHGhsL7)X#JxD+=t48ei<B|%H@bj$M<Z-E
zKYz5c-T{Qjg7&;s`~}z}?Wb5o^EPg@d#)((xvC_~eQB=y&bQiZNw&uhl?M;-K@NIe
z>JM0BP-3NXhHMu?x|e~?rpim{z@oTI1<L)bAYqA#7qJ)mwPeIfX0v+|U-l=++e160
zPbdL_QTeMsh*I@;ROv<n#^JApp`rLETbO^2AMvMoh<|!SfD-34h&?m{`|Q-ifguA&
z`OdC7Q}E3$@pBeeMuDxuFatvZuci|REh?^qjdK*^B6JY{h%hcRkz$3E@@s~|Mf05q
zb4FwjPM{z%EEkmG=~jbz3y4;dn|Ry;xn&GTmSA7<&t#p*^`>+W8u08Tagt;Z*hps-
z{foGG%ei8y;#1JvL2NQXrlFZK!nqmc%)!O1la}pmFiF;5tS}8{t8$dpL!uEkvtl!c
z`OM!>R>)!Ml1icr4MYo7FJfrN@nbpHi_c&;Tw-cD`HAu~N5@GHj5(h43VG*t7t%iP
zUZH>Uq&`jDBKaBC3w>=+5x38?SC(8wn=|KZ+;ay0kvuu*X{kNNw|{iB)}@JO-jjIX
zDw?qCh~~RV(x++m$(PRdD->S6c)#n7_0BOt*mIeO=gjvJU%#1GZnvhbf{EmmX{sE&
zf)|5??@BL><vi`qKXv-$SDJISFPO1wKeaf(_rX|@uQWx>O@u9s=|k|R`A#6p;vp_e
z>r_(cnY;t9-dVDV=3-TrRP*KC4Qet=Ow)vMp*Ls6miant%|188s=Tlfm=8g&H+r+x
zqdm5^Vf68*IT!og0aaUK&PIIkSkqBpbv5#iianJVBcxI?o@lf%6a)0(WRW8P<i*G4
zaV3hG1hrt~!u?nt9}(rI6%47`P~@^>#k@n`4Q1#zE9JN+(S{qW%zyR5f%(RTU{-sE
z?dW~6m(Jj3tm9t4pqcskA(zoaqnV^dM=KiFz={Dls6n+&PT<wDU3>m<(|7t&5OY}D
zJjwO`-RSL(Tn`!#X6*T%>-+R|l!(n`J#CCY;-exByMu}Tjk$Ib<-h(MK4}Ejt);Z0
z;n+pwLZPTNz2_&VsVSfW?c&d^Hr)pQtZMyJ)8OUoC-s)oC+bbIO=N4Z<uAjkrIlRu
zcs5|Q*=6HMv>f#^0&7)D8O~!7!LF5df_A^myLvDk3fHy=e(~s7GjnoQTE#c#SuG1f
zGSv>hve|PicYyO2^=<DSQ>bq}fv;vov!W60c0-sY%Ye3A*PH^H-zm;YFBBF6!E$y}
zW-(oirhH*EM3lk-zkArS>^}Okx1XU(O4`AihPO_fsf4Kn5r3N%0lGG^%x`bs=mc_u
zpVvmESsleV-^7I2x1He3eOgtNRIJ!i-ys=g^DgOlvgePl&1UrZOD&POq69l44{BZa
z(YMuTsFFgf;kA-l{pqmCb^s+Tm}L2MX@fHW(SBVavgvu5CPGsW%g{*3>s)Zw*c692
z9|c()v3mzlgscN$6DOnA^MEf3THBfK)DFDCnMxnl&&_tC0l#nlsd_A!?wxOe(QH>e
z+MYK<%>|h@8=3xG8sG$CpG-67{tl%i9L^HGP@K2A$eyJwxvi+P_&DYSCTYOp*Br$A
z7<Z6h>nqu1P(5W`J~FK*Y+M#3C81xP55Pa*$145WoCgd-XEQCN2H5x1V|%ez_J^Kr
zQnw=N+JwGM&LbEzbo<b7rh-*3AE6)iu{+v^Sff{O0#L4kx=~x}Y5)>IOThBhS!^Kp
zjD4Vzsp$)bdY$8n>!U80(YA>sh45;<ZlF2tTlDT?#CM!?QijF;7c-HM=zpbPyhru%
z)dGLA{{Md;+y6|9ZM^&Q_*&`Ix4viX4}?C-_<Cr6eRh&jM<~EnE~OY31rG*V6P3WG
zA(V%S7S*AIM%>^rE=2zOe=FiQ+GtF?XfSzxCNVY+=Vw|CCL4(O#feB4z`BDR5r|hK
R<gvqui~)->v1ldK{{S6dQTYG>

diff --git a/panther_lights/.docs/ERROR.webp b/panther_lights/.docs/ERROR.webp
index c562898380fc93781a37a9080b1c3c010ea84897..cfc5f13131f2936bea847a3974034ca8feef1c41 100644
GIT binary patch
literal 19200
zcmeHP30PCd7M=jIDVw5J5sk$KmntgOrD_0MD|{l+=TfB>TtTcr6+}@<RJP*sv`}};
zQfw8MqEJPvCZF2cO#zEqMG>(oEeN==2+6!T6A}pAdx^m)?>*;(<Yow&Kj-{snKLJu
z>^g2-r5i%iKX4IF6Au5#6d}}>{BF}n?GSRF=<04r9$_-zk^8vi<dI@1p3xN{J@S8i
zZ!#HZg7)zGZr8U)W44S>bnlsL_cnh#N^NVh(ErXdo{dwx8&9X6ZL_v>iu<w~11#rR
zUYWe;Sa#q#eq>7QtYhoLI~fd}vh!j3$NZnWrzBrnoOH*~sRkiA-g%tgGl9H3T|Wgb
z=uoYXVEr%1oOQkRU3R+b86X~i>(-Kv-ffJ9sQr#Z<{@U1>yeM##x2cQEdPC2%;X_^
z&IOduJ~esE?h+eOnpr{Ih6jPUvh4xZ$v*Ez-o7s;i*Fr-Tc7u6e+zFgnihL=RK%42
z1&5^3|6E=fwK*kX_tXq4+nFALkJ}5=Vvg(C9dIq_8N56;f#QQ3kXWedkvifzWIo5Z
zPGT-QCahH<wo2!XUXVWG=DT=-&5wOLUAl9;+99vc_)A>xAS{94-7;-tfY+7e%ROpW
z1q5L%2o{j}bQT0&NH1+@qDJegBv+F~0Bo@P`?M%1HL_^X<8Wlr--WIXC^j@kL75Xo
z=7se%H$HK~-ulc5E<?S)FFe3N4`;Kf>1LsQ?--JcUOz1EXgt{1Z~xF--wfx?0w7Mb
zx6J7L6>OIpS=_EXZ|V9dC$ZkZ_j}#T8xBwa)2hapD4XVy`IoPo@P+bq1^4jtFD0|A
zo#@JdVKWN`urxLdL^~@Z4_=KTEG<5_e6O7vQG7N@X9TG+S@E1?kcoJ#%;=}-qUWC%
z+nsq{wBzj&D|35vS=2p?->T1z>xq`PHmPtOWWeP-K{&o6ndezJU@I$^31bVrW*R<u
z|EWl?X0WGQCn`J8=SyeRDvQ-69+xZdjrh~?;l)p`{P^_+|G3NHXtq4yUdb9MKm}Qf
z;erlzTk|DzY_jy$+SWqcaq`KJBQZ8#eWd!jb?wT4Lm@?&P{*;1%y7ttU8gL@e^yFV
zx+Fwi@s4#2u3Y*o<k3^FgWHZp*<Rjy!B!9b>!>X+SnLEi0971Z(4j&_EfB1r2@AA$
z45fB8cB<>)+z;wGajP3l=0~1(0NCbt1*dGf&|84ZoWuAs=Y#X7cHU_5ZOns(E~vLv
zsLS#PdH@Ho#ZX*uqryhnw1muXNG$ZCuV&6%X(lo!0&D(?T3C<FMV_LY-<-&vJDNW&
zj?X)AKi>&303sK-pu++o@<SpWMbwwv$ML{#TOvzx?btIxFX$xF-;U%}wz<1TU+}$M
zQ{a_++x3)`{1sj)lH4nX`g`*ylTX?F?>2-?ieA(MKLer9h`{?R%4T7_@ou)o3Qxbe
zb_q^K24?{QpkeUCn%J#&lAh)*x|TBYUN4M)@sK~3Tw9j6DB$|AA*YQ|kIEZJDrU!|
ziVayDu_0teAVQ)1_O}>=KqIo&WgPqRk-XwF2T9SXnUlW5RX%djs#$+$2a9>X%vQ>Y
zi;+6enyWrO=XbC`f}c2s#jfu4Plq+jg+azVn`~QSB_gg!p&g=;BO3+}g`;T=JaWE<
z@5pNW{*9~7OI**yu8`%tGiuM@7hTX$=cA>e{iX<&xWeIr4t0KiO6JsZGPMlnp%m$9
zN2)PB@uSwKHy+3Ms~yi<l^f;MuFk@46_eES(*{{a6=Bj>*OCrvl9#IT4I@G$Ja{86
z=(y4*0Lx4SHznBuBa2l*#^`PqMa6<iWpP}3Gnc<W_D8d@Xt5H{x;gzYcJYv#2r7HX
zO?oD}wIV*(uniwT0j5{X?g2TtI^nnBUYS_dE3<N!py1QWip3;CnQwPeBI5Frx?oX%
z8J1<u-|d)-@xCFLpINg`-~m{g_LAIav`d@&maEVl7>k8Ia_9K8^Ih`x84mQ%Q(|Js
zDk!%ht4QVWlFN8qZz7H)x<T%&ben4`!3)@*A;5NBT=*fzR}wM7HZAJmQwy<~saLI}
zAVMofHw1e|h?9^fzD4e9^F;St!wHqKm#(kjC*gMa-P5dL$2OAuJzWptC?CO&(Pg)r
zo>+{?mQ85njf_oA$?9bh7Wk*)@7p_LTqMR1c=(cjm491;tI0^lWmj6p)gHg&nt)+D
zuM)rGU5cG=%hyRR21E>TvqXcxCBPM!#*ur)P|q3$kjb1zSp%;U>8B$3lVmJ!@Bdb$
zpQO6b=!aGn-jaO9tG!|?<@b}+1Vj(?dxY`PBY5vz>~^8-RM1ERWcH^M!4H089=TVH
zy7vd#hU~Xi+x@ryHSpGmh-kTo^egJ;9LV2T-}~d>BN%sBGRQVkBsVCP?x}1iDfCvX
zwOO$cxmCp#%J=1DrfI&jrmS^YK!du&Y2bV3-?04H>En??5iZ{<dX|Wb#>%~B@eNiF
z6$6~6d?&YW&@SG+b+Hao7FIw?JbuSX1cIfAg5ZJ<4Q<da%@S;I6!{-(eI^@pAU0^&
z)~xDFvN@0!{k#J|zfqpoiP+&{3p}%%JY7(nMI5#=y6Mxkx6qoAGEW9f3M>4AR)p%;
zH@vjZZrgr{YdqtR&+3kG#&Dea8TP(gdF{{+sn}ptsu*AZCNAdaC<`!~t-^Ieis_I1
z-6X?6J|aoE5*K)C#n4bswZ3u;`KKI4U+K_B=!*<m^&wg+!SU6*)-QdUfGdo;m75ES
zyM&c(iua8#*x^*CDZ1f8v5hnpMB_(XBTX%{B6%9GP&|DJbFqPCV@m33p0&i0EnXSW
zCIRDzU`qIElq|9Opk5eA0W{Sd=CVoTK1gKHJ|r()c1?D1(`TQS;A&sdb80#h`LYS(
z+O-*2Lj4u^7ahb8;#oizro_9Zr{P0jM^8+@lZ8FYD6~YEQ~T&kYjMSqd&N-qd$3Q}
z5qrA^*T=Ts2T#r{ofvf?<l~E0{^97ILsGy4`kDh5bZGAXIb+}BB2i7>%9{~~aK*!c
ztM`_sOLF*hrLD?^>byVI4p2u2+OeBTqx<y>b<zEgyuTg>ZYsL$xbnJ#0dhOF-z$M)
z0y$_G=WqK!oR7GROTEt&cf7S06C;6F(>BhVwl@2Mxp=LtcQo8Vw1ybhI||qM|2An#
zH!RPvkOy|F$ro%BNRhd5XOeVjX_vL03L*-dU2XuoY()T+?Hc>sx_Z*Yb9E`@y+s?n
zj}YR6G`3SI!;L!s#bicD{3q!*>`ww$VtGPlO}gNjZ`g?yTY@%@a(Dguea}Q>wtl(+
zpDa)DwXB_rw%pM&3)WyK$5mnCvRwB4?ul%sLbpR+?|jDnuJPH}DgHisNH0iLJBbG6
z(V^lL^(KL2jti5PQfjg$z<`J5wt%PU+?M@7gmW*)QaAydd-<{O<<yp5F}pRW!ZB!0
z#iS|qcH#B&-C&aM8YS^&{n~};ppng2lhaC4LTjo{$z^TnY}I9LOK%$1a~ZTo*$uS<
z9eMC3s@@EUsMd6HzA$G!PT61d=m#wCo>>%79hX1muI+77?Xv$wk$%%C;zVSj7<HKx
z?8)rOB-{Cdhv4v|h2tbuI|Za58{$A;hs$zv*!92m+&p^9-H?14tr-u)$<urxb<q4o
zhoM?UCH{KPz`~V6Z;)FML*ry-Gx`@8j|su&u5FBkO307QvuTd=wDR-ub=?c~^=h9%
zG`HdaP98qP2>-Qhzo4IqW3qIhPZpo71V~aCkZ5w&tF`q!p7;w>Gs9Ua0UBnbI--{F
z=6C(m-mQ|va+}cuY+QCzoPZeiLW-dt&!Il0mF(F1LTFco>f}Crn2$GI7BDb)Uh3sw
zPb;h!30`GK0|UP)r-0wuL<93l!x%eHCIu2r#dwtT`9>5Jzh7?mUgx)O8KZ2+J^zFx
ziOE_ZpHOnI80tELJ><*R5mV<26R%r#w@_U*U0r4~8_KMHf0Nc749eCjHXqQkWkrif
zlwc}ZQTtm!=D4zqX87_3Bm=oIpr959C^#%myJI5O?kcXVl+D&i7ZGbgx`QNbCS$U?
z53+z3+gYXNpS8@4*t>9EQfC!I{ebe;zgfJ^II8M?Q=Z__R?8evm|E~Rj^QDDOE4VZ
zuNW@qP+1Y>FG=@w;H4RpA&R4A=8IZ#Naxu#11`!Ts9kHFdvF#Hnrxb)T69rRiQKC1
zYWk8Hwq4D;nQywLl<B(6iCC+KyKF9>|2irW3>Hi|2|GTncpR*K?~Mla8U{<D{;jD~
z9@ExUy=-2n0q_21qPqrNQ=vwiqB@-ad1Ke|2|V;c_2NJDz(Bz7a%JkeI7MbD{!uJk
zOnS;zL(u*Vx5?@#!_JE840cvrcyw7oehuu_pw`ZiOHfHcE};t+{r7cE#el)lp`PEs
zPD>XO;%u0r)>gO2d$BHnkpyw`I;I$Wp(<<8k%xZMHAQ?qRHYQ7E-QM4T~ivxJ1tMT
zH12BP;$A98wX9j)i`=bK)EZy7C#SY|Y79}RBV+HBu+4}<q;Fza^XnP1_k!lBrE_<C
zz9*5T3uLqpB`r{rE>>g#5b0W4_tXnZM1wA+q(XA`-`SgVDSacT?cb{2ZK3$No6wnm
zH~M70LL$0YeTh9;y@QakN2>Hmz#qLnT9uej*}%opG7gr8%!VG@uF<VR+{nkz)HdKv
zeb#Zff+KJGqyls4=U1BJQFQ6)UUQGs%|T|tq6R%uJBjyD3{`GJ<M~=LsiS!Q3j3s>
zANtRMv>*Dalv*cOF2}Vm_}}c4;@Tx~XSUiEpEdc6Ld)8Zw9Gvlm)^KotjpbPORE61
zr0NJmPM}eo(RBssHLzQQdK(v$rqs)z{-67!xV$8n2hehl)<ka<7mtMXH1<M$44h{P
zO*KeCeblKovuSveU{B`RP7>bqM>YO}kh4-!mDcz>0>mn*t1?RJ>wL+44ofANvnPqO
zuZI0J5HeWBP{-j%Cbhz$b#IhTtb_A=OX`0|M^&ch!lX~O!6%AsPtISO3NiufXbJsK
zT%MjyN4o4eXc?UtufOZ*u)=W2t;%-ju5c@v*{tTz8+=C-l13ojjMMD{Ilhxg_1Sk-
Zw}DF;aWKvQo}{{}xC%A(Ji6_C`VaZ>mVN*L

literal 37586
zcmeHQ2|Sc*`yNXog>Vw(@YyAiB~BcMzir6UCZe=B*3WWE4Sh0{gf`oWk}RQ9mI{@F
zDEeBpkc47T=Oo40$}}?X_r8;H^fs1e{xkD-cz%=l&EtLNXRhw&y6@+H?)#cG7&Ehb
zaVXRVQ)4@OJ6(r4C=`kl{Flgu5<#JiY%tb5;H%HW$i~`?lY{!|^A(}s<=}nNo#1rL
z98?O&k|Zus)H3T8$=2y|a&p|LRaRD5j{W+Y=rRrxhioU$9_Om|2f^ib(ULtqaeqlI
z@cy$e+>UTCJW-uk=%N;8;M$vAv;3=vdJl>`tb2_sE9Lr<H?p%n66Jdq8EefK+otU9
zKc~dvMo0)>Fwf&7+@=lDjxRX#<J4VJ_qlKs$r{byEpr@;%E>$|_PRCkx)1MTlTDG(
zn>3)QpM#!f2nik-64KxsK}d9i&k3VQ82Bi}W5Pg08;&3E=Hdx-3k&2L;54s8EsOVx
zv0VA6n8W|BuInic8*#<C@e!CRxwbgU#L}E){MzXc+pZ3&w<udY#LlZ|jOevA+ga>@
zLXn4@x^(gz_0}q1QuN{?sS+_EM61C{mvZ6Zo<u@;MX>x(n;M?)TP7vxL4kS?gvMML
z8bTvz48YLPh{x>Xrh!I4!md<*E{?d+yJ*)m4LfnAxd}X$T=F9mAR3FtMT7K#kVZUN
zQDR$1EDL||mh0aBN|ois4m<2>`1B8O{9@GhL(+?-KPh>HpEeFHNb8BN_Fb?zLCe_o
zUuZ|WKvOpzegR8~e<{YKE&W|zrf|`LgYxr);_p}&{5y>L`RI9uFyS2slO7l*T1>zs
z)$fn8#reRUE>cI5#&L@|YZ9`QEgAa}NDmIwd)RFwA&&?<YUFUkChyF}_cq&qFbvzI
zfc9TJo8vkIKZ^MZKwGAJjDrb<PoZ@q#82f@1D_QqFqtwL>CzUc(0NgwzOCx+5guZp
zhG@CHXwKIgH?-qScIaUwGmqAqd&(BYHd%DZ?T#Cn5E$C=EF)jYRp{v&?<+O_`=uk!
z$8WlFC|oQJMJ8GKfw)*)0OU%0`W;uObw)iYdb`)6GUTn<w$;Y<q}cG``olLf4&=R;
z<r~Tqk22+V>NFD>+^8uWz2IuyVa4tuH+fN2zjF(1n?0&6l{VtnsHYZp(2WS>ai-2C
z%vd*yes6U5#V7)ugopTZ1Fe7fFE2Zlbg^@N<#ViW_nWtidYhlr>%1&Io9lNQYwLhN
z+I{*`g=F*3+e7q9_T2h}4S0Wq`1NIHWO^Vzq<27=2#v!fMDR_=dK)HS^0CX0xWXd)
z6%NqD3Ax-9wc1`t2=hupV+#D9)SX`<c|WhJ1|y+^p6iCMJ@FSg;gQZjc2Gr@=7VA7
z(|&JO_2VqvlSym!zxw5`3U{4CL+J$tyl*mF2^iyr$H7JEkPb>6=4{4zpI;pP8}kJI
zcD_F3YMtmcnkkjnQRI&|2_vwyFD73B{;&><2r9neb>)bC&G{y6C$k)nr#=#SXVw>e
z)09RW<9<hA99#JSVpcR6CQTkF)anJks4Zh)_|hwL-YaM;+_<=V5w37vd&1$@{?CV#
z{6sU|^$vuBsDu-UisyD51Bfm9bNV(VsQUs}ieEpfPY^jb_cks!vReEo;5a`?Q<7XD
ztL~;Z>}s~98cVu!_(l@peXRKY<@mQUnmpSp>P_6nT%IVt0^_Un|8FUyOlvomX-{G@
zWo2kFZFVSXmN2&C=+_iW<y?QlRn<L9+IWoStyd`Z5#!b1^*X2h+@siN4;Zz5G$4=&
z3lh8mvLg_*ITA|@Iu*>DB#>JraWs{?`f#Imo>cj)?X4!)ceyRB3m_`Ey4}t5817M#
z;#A<uQNMNO><vxX71rM7gfIu<e05332yKaV3}P~8982kAfu#`p)kffOJ67wL+59f^
z#AOv?LoEI*cAm<u?&h%Dj!m1YEPG3@Zuzx-ccOo7nQvFa;$10OFSIT!D&0L>(%lnw
z;gyHU(&+CQk#aRsU#f@?0O%&Z(W&(_h^=mN({FAMCH(hcSpMZB?MYIkLTofwh1g1-
zl6sx!Ex67HElL8PoI51mI<zQ<0XaH>X?ndo)4cPx2WHPE=Qg)_u0V%=&xm;X4F_}B
zHUhxwY@8EL>f=W1ic=zYb`@10k#|V`y-)i`vom+<f+UW~u72gEf5bd?3R8!-FaQX}
z=dhDsiK+y0c*BQ`3SdAEXpIN<*}iCXeUKoXy><B4Oc&p3msyUOsH(N<<l<Vjt8Qxh
zax-tXolC+wdx||T6uV)+W}AOqwPC*2p{~Y5tvSleb-moAo=KQ~=xk@m?TizlQ1}&!
zP*}pRg9I1yfkjG5X8{0HNLF}6Og1pls&tn5$+S{7f!-%45~J3_PL3Un!Vf?G<&^Hv
z0AS}%-Y7*XGIZJAZ*!`@^EPMQY@%y-vYk|0e2EVl7h&_}G6z{)FMIOo?Pvxdfh>ic
z>u1mqPrl4sb>aa4IL`)D1&23E-3awLNz~ukoOP)oU$?~hw#GFeV7JQAWr@qHFW{Ob
zqhB7%s@|!BA2i=q-2P_g;(LR`HSh~>MlrMzq_7G*?c~Ef5EYH(1KR-LA{IC`8_*Se
zvs_Z(r(2%H2M)&f#C3kt*b(0$cYJ+~V7&HLekIqIb$9RCro}mKFfmy5G<<1#e9&4&
zWrKZ~Up5JS)U+2Wh_HSI#*qvR$WA88pD;U){b1t)A@V*302&>D+Dkyz_Q5Udw=B!q
zQybH1D`;`3CU;ZDLafIXQsOqE*!pXp7P}M=W}0@?FRPiWSLC$U>QXTO`Ie&0mGM=z
zG6UXX9ubmHz|<Yx0}%}S{=Ey}ggGgC2M2U51v(vpy24Lh0~?Za2mx`vMSQCi_b79F
zMTC|%%O&S=uH|cqHj+qBTQ-PMc#BS`#Ad{p{qDhktrk=ASYAD)BXj<F2~I-55ncyI
zBYSq*$@|-Qz-jaIDkVdpD;;?Ie?a}g;gzSJmS8LKYfo5V*2dMYdsXz?Ppa}ZQ+=Tw
zO1F`KH$rv-&rDNuu~253+3V)L4jx$Wy6c$*4(Jvze76|apgY_q``oj$a&?6D#klfZ
z`Gu~}`BbL<vH{AL(mUAgZ3Z6Wf$P5IsyhH+P*>{n<CfT<LH)sl%3hl5qRyK<mGO$z
zwm6w-y++}j$do3~uNvix)96J6x{ux5I-v3ym24Ma&=nY>Jeu_jiiAFyxyQ+B-kpEq
zRb+LaiCd72rhcsWX^X>;<R<>boh1F`X!;R>@Sx#+DbXvm6ONf;cHk3=q5MF9A08+l
z;OWBw<lfY7&EZ?PzSsdni8lF{kJ93LUq8kK_IJpY1q}#q)+h0){kWGq@tDCrtJ|qY
zv@YOnq92dREo5NwERx`LhVn@LMioGteOu&aLtv;ut#sDlmWl>NZ!H7OW5x$lUZO7o
zA{k}p9;d$!e7w}~X2*pk0ZKtFTKs~&D^>2L%=r;}gtO|CO?2N`>c#233~huV3*m<4
zN~A%6q4U0X;4=&G;4q}%;g$oCNgUlb_BN;yZOGM%UE;G}_Wh^a{P?A&-CG<pdK5%H
z9Nypi@n^j3eTObR^`$j|B78$;M#N9239*0X?oLTs;8Di6pV4eI91E(4VGS02x;UV1
z!>w1UegF_sJJ_^&$ZFUryLeWj_uQ7#-#U`e;Qt@JgZ*~V+Xn#c56xr?!3|BAUXNl3
zH-Y4^;rB#ibLR_c-#QkD(hCH5AA|*bXD2;U9ROvXS;V5fQdodk7#sPn9|yGBd{Vw}
zKpQ}j^MPl@3Ok&2m*fsMRa9;;i*(S8Jio@Z)E^~ab#T`D(?c7GmK`<xJiq>6xhl~Y
z#c|ia^?V`T3*d9W6xg?dAA{Zjp#qP`ptqw|3NW8JdIbrifV5c$Tew8=1H2^R9tS~H
zZ2abp1ckE8E!IKaWheWd1!@18-{*aQ)2nC4OYdHw`D<Z@^F(k@t3HAT<Y}3YMo%|*
zG*MWZm6D1B+KoR^&=AUA30zrsvmqhGTtRe&O^vyA#?fR`$raHneMAGAl-?Are&TT^
zo7S}Zo`1)^umk^ax(6-$fZmXG_R(`s4fr_gK7YPh1<?DOfyUWe>wrT&<bvJ%M9U&2
z{G!p1Tt5b@*$<W&Y@M^&WS-7{rQHjH%f)j3(=4TaKuv}xxjja+(ZGfO>9y`-iLU~;
zh0=`$?cK{o(C@Mkjf}t1=;I=zad!lPW1dmipe$F*JHN%M$k?0YMmY)>$WP&=h;@uK
z17Oh13W2MI)p03;3<O#&aQ=WGhU{a8Kd4oER79ZtKJytB>HdX^25amCVc`dbL)aUw
zBP&9s!5*yUQ{-%45MUksh}zdqlu2ejlOiwY0pGa@jrdyDcoqaD#3(CeBI_ucJR`<X
zJ`9ODQ(;&}473gkmqJJ8EIIK<WO|*}j&AI_VYa0p?*xXO>jsJEa})JSFx@(kcZAD!
zZbjl*KRlC4<yI(@$`WoxTuBJTt29ud?EP6$**sG@M62E#nH6U2WLN~2AyOEYooi>N
zYIWSM03zZ_oGI1<&y!FiSSX9e>==bOQwI<0szGM21Uut;gf-K!onevNcSWOfk&-2V
z`T~TeR9S`QjM5u0hqtv-d#-lJ>0+NBzxKo5jjiI`+Urhs6*)^^gSxJe*ro@?vNUj^
z#Ig`B36o`$K>Yv)I_AIIj!q`yLPBcy??g;H787QuD}2u#;>^I~yH^qzx4)G!C4m{m
zi7zx-5UWZ8Khu#0{SCFgW-OOX58WCv_~}27&w55Npr8kY#$Vv1)~FYz=oy8gxNjC+
z^<Su2rNw_;DoxlR%{{ByVCl2_fAXf|1^Je3?R6`6Q8E=E24Z#!9@6e3iu`3O;Un_(
zLl6v0kRxV9>%2l5{Pfe;CTK)vv?EqflZCcY0Aox#qi6J$;uUGeyL{<gkQ#-|M5>>?
z3G@?U)`>FFW4-M-`ZSE}@({b55gn&)0PLTdaz=mw&8zQr_Zu)8j~MujV=ps$MsRJx
zf2^L-L`+*WnWZp_3bLe(*neVyNG&58Cw`PTmSr^rh&#xEJThBPwh(vN2O&bkPAI!S
zR#a6W_hfjY$c@?jiqskccCH;{R+_NA1{RTJN$loX#1*emA{2H;Y)GteWGBNSP|9e$
zd;uk9gY=+TQum3lf@z3J;3pQ;TBI30Bj`ICsA#YvT0>;R1>3n5iM=@!t(k$zvjdE(
z_aH~gXj1E70UC(YGJ?nP(yUh%Ku+T)#X{KIBaNJvV94LXu>h@uk`G{wx)b92#VJ@!
zUo`=^XQ5gLYkGo_rZxf3ovE{4@d{y*MzkV!yAcLqjg25ku!iDW;45Z&D;mMk09VXt
ztr7EMF{IfBoIgM|9X+FonNoFpb|{0oqu&94Om81UBU+#4F$g?DrRG4UV}uIFg1S~%
zg9Wm8F08c+<ZNl77!2-NKrC48S&$`YL^BqnS_o?*(-}phzrQICiCI$`Ylh-!r+020
z9<h&IGt91pK%SZrjZ@2Me?mtC6Uw@QPA!>kAv20bUwsQF5>NZ#yBI2qutHG+ne`wu
zwL>61d^Mohm7TVN%sMl6GAvT75!o57A-8UooofeqVn*y{6U3FkQ+B~LR?cW{fF)IC
zh!Zq|?1Y{5nKODu-|_dzkM4B>CabKfL;WK=^k{U<Q!ttCT1GT*q44atXkRhV>5AA6
z;E9+vVayt0hJMeCp3y%`Nno5#XofoHXVfOXv=O8#lEC*m(qrmazt9#;W{OhFkW-gV
zexu%6<x7fQTqIQ@CWL4;Sm{zOJlvB=2(Jj1A8J#>^L@*t<X7pCZ<v5$Sh${%5GXb@
Iz4VO!2QqL|*#H0l

diff --git a/panther_lights/.docs/GOAL_ACHIEVED.webp b/panther_lights/.docs/GOAL_ACHIEVED.webp
index f194ba38b6eff3db692d1e5c224d0dda708148bf..2f862a57713197d6b5bb74f52573306f58e550f3 100644
GIT binary patch
literal 19664
zcmeHP30zdw9=<ami-?R!F1d}nU>Z-62t@=Gks!&fLQG5}Tu@8|L}Lczp}62eDPqPF
z=Dzfk+(_X`s67prT%MAOJDDITAS!U)`JaJd2G2b+TpY3N{%E{&HSRb6^PT_t%^fwu
z)%Cas$9*`=*=wAa-A7G1jx)#qt<~dNa-7rqBRmc8JA@B-=jj@X-zg5ozv#$uV*LL}
zeeuOWV{U`UDqhcm8@$Rb*|U3U*Dg{wZnwGd)S1_VMeT;Pyj(K&NVCW{w|WL&?kkxh
zIX`Op-ZOy<rO{jCChT1r)mDGNn1q5fAL;SVTT?Fvq+B-`Qo(UmC}E$pdlLTj<o;oB
zLJsBqIk^9)_*$A#?fsn-JjME)NV;ZCVe5&U`0V}}7ZweSzIHz=bYs_df44<`?XoS~
z=3L+R%=)UOTTX~+#--?Dch@=l1FBBGvt(5N4M#HZo!2=2va?IK?*=Dx9hQ)Lf(4e5
zGk8%)5QMLks3KVB5m|%4S8`f3)yH<a%Z993Ws`mywd$L~cCtODH@;eSH!!zyt4Uw=
z3-PloQa>LUoe7_QMaqHyJvMCVz0x&%eZ=~hPgA9wU(BYm5XXU=^Y9%NR#BjXcj}nF
zz*p3)qJjxqCG1!K+D}YFRt-dHw#p$G(UOr>J9?~|SN6-*$P?psqz9PBi@6b>WX=yT
z_7?*LvQ8{S=Xk6H>r^r#nhxqo)idhyT?8XVO+_1GH+7yhXqL_X>21!MwwM{kec38t
z!#$rAc@3gSI3b79MufT3@HKVjdTe|_%#ECc{`A@5JvQTf>e+6Jdjo?gNC<}k$qQet
z`3300;TZ+xX3tV9N8WY+lM{82%K-}@iG>q#C~ZQJ1mkNAB=e)U3)oE|p6Ev5&$HvQ
zD$j;*DyceO9+l8uG{D<St{0O3FeKrC5dZ;Hs|g}_QAY)PpTQkIRQpZTcAE;P{ee4Q
zgrH)V^#dc8p<8nfS-F>9bGGhvu=j75Eica=V<a*$xg(YX6oM?8oWYAavYGfw2_LK1
za$M7s^|M#jtVgiZrkzJ+-y3(8nxR(Nowr91Pr$bNz%L5!4|a-&6LKig5Jt=KwUuhV
zSe?;xp9FH;w--}(Qho5)?3FHKN9@cUR@YT0fwexHM<7|s&?xKd^MRe$ojPIr?UW}e
zeNM|;w&I4hzUt;Q8z2F`?QlX4B|gr{!^^ETz9urWMlD)6d${CJ^fq`jv_vMZ=r`WI
zEny$alTO6eH2!lyGSzke+GH)wmseYJVjOy!-txMm6S@(a9vYJx{AFSD$L85yQc=f|
zD*+~8X6;Q26`MM%cv9aM1(`xa%s5|JRr@U^1@~o@Lx)<d4&tnns*x-PNaU!s9z;64
zc1*4r7rwd2yS-|BQDHna_?f`Mj=UjznjXBzVHbMRE$l$_#;F@Z%JU`SENg%OmSOn{
zKFh#rnY?$VM?30*&AR)H4$}t!c|^!6|Ff<%*iQCrwEn8N2Te*t4#jdq^alK7W8aj3
z(X<CxUY)%M>f|9#rek}g2edZoXPAe9IbB33(qx*0^&c+i-u;nL6|tvsj;_ApV&lIw
zNNP~AIU=EXGcX#ln$T5jYGS&YniMEwaorS#tfKvg2rZtIoU^EZTT~icFluf|C@0!c
z7Add6BP4qvdrlau`YlIj`OO`N_m(|E<#X%{tcJBZ77cddVmlbrU?L6;-qi7`Gw>BY
zW;JBb^*x7B@lT;|+)D3_(B0{i7Wb6H&B=NRwhy0AQlyiV<f;@%{#GSIw>Fhtx!u~~
z8Y*(~Ie94lVNd-t;7-L!HJB*56!G(r-4&_$B)4RBKOZdW_`6{q$MyU%e;XycR<hb?
z+Li&i{`;Mjem|lduvwKM^l<d@@R>OIFYmPW{WfxZKZTW8ad3<ZqPku(99Ij%lFnX-
zF_=?{S)Y$iL=`SwoIT$nV91)id^A0*0nK5@xMc{Vap?Fx@sbMLP*#gXHCQbh9@|V@
zE5at{jcaTUbsdY)$us{tDeECaH-nv%%NF?A>vJ8f<zOLAKJv5*qPqOSKC%=3CA^u!
znCt_TG~rBOWC_qm+B*Z~bRTLy%LVVY^x4tRZWkv^9(wcL3lh0F{KU~k>*MpP@v-)t
zP>Z>{(jDcnObo64HiV(YZVCyOVQBmM6^uMGIWS+s?Yq+-Ln1aK2?4yR<CCEN)r4uD
zIo)#?czqm13ck7l8O+t@SA<S^T#R?zWrm(u?ke7A<#cSm!=C(niQedCao`FGE?4}g
zKKa4!6aV1)o3N`d{?kGX^{*jkh|g`hCy9d&ipnvs^p0~?#h$w=zh2N|c-47XrBQxz
za-rh4eTDy};!w{q7T}ATJ|#7z*ObqE{?2?jLdiXS^U#+MP{p3RMVF2~8Gm_R+)m@~
zhK^6>`q}v}k^>%Ktu9zWN_#$Rrjr0$UR=?L3cQnUCpt#wB%jsiTFJ4!t1(e36XJ=X
zGTGgbouuCPP<m#=Jj{%yhY(7=bYn)b?Sd{D7}o+>Pt%nX<VM`!{0IO>tmQ_|;Khvm
z)bIivlY$o#PJ++t@S<qOE)>x0UQ|Zz3beV;!_O0K<&9*0QKAfO?tiQ8!)6sYCf@On
zzvCG5g<jGS?uxSBX~>=w!q%#%n@}~T$}t%VOiK@jE4bQhiXAp5v3+B(qN?l)V7s0C
z@O0(B9noF$W24j4zI`xRF5w0Z2VevlR8oqg69@e~U<FiQZSbGf6_{tPfk$~<&ZP)x
z3Tm0xd5;Cu7=!TNmk@6v<)EAA{EfR<F6VGN&Dzk{ER!?phwG%O0?%vc>B7NIiPBT&
z-$PY?F|m`&e+&wQdo;%W)q+8+8fJ{j{(W(Ii+-*Q{b3urevoHkw-pK-J(4e$cD?<;
zP>c=LpuM}^MX`4;bi#YrbU{W7c_Ud~3(px3AC?@-NTcjkG$aR^^ywOU3mFIMDx?fI
z0dFh9%`u~OH{B}+9>xjMXVcS>(GY+HY-KS+LnIVd8MR8Uj`zQuQp9`9$9tR)IGvU@
z3Ho8Byc!iN+Mg3WAMH84-8j~Ny`pkqM$&`)ft8~jpZt;V$gun)@)@_nhb4z{4YcaV
zK;|lVcbKoD_t-xOtWLy;>pE+Os@@5{Ghs{a!OZ64pJw7a!m8*ZKe4x`j{r#htX<N|
z#~8wB=$+~;7tomNN8`IIj@mT_k3JJ$6ugEtgNnL&D;Wo{YN3~p=GCebc}i%)A4)bu
z_=6o6a~{!v&Xrc(29j<O3aK*)N)QW1j1<}d)k!x-m`~QKTlK1S^jJQUHIoq7ZKzlG
zla(2-RN)e(^MD6XN8fx%A5wHlK4YM)kyN@@!=YT#u5T=K6?N5Je|6>#6u(3|4B(ge
zBFJ3a=Qm6dP53X*++n_zGrfm(6=!MX!>t@)26Dd;l{U9D>lA#-Am+r-laqT%L<iPp
z;=3wxf$iiBUTXbPBTke2JeD9BOvH~z*+002iPwCPyyR>Y*Y#FwZySOX@)uV8WBXFx
zE-5pg!UMyK)eb&cacGBg!EcWX%O%|MtG1zX-nxK9oFI(Xi?thzxL!74Y8~QQ5l2DK
zCF7qP72=e<y&UgF_1UV!3j3tq?h<a=)M-i!vGVT5n?SR6BLfxhqA?RF=7c_+SPEWh
zSr7S~AHH&qXX5PJB2DDrm!^%(rM4Fqo`lznB3K}~3ah8hheEe%4hnCS*|!ER3mk`N
zlD%l^d6e+2mX5VSjjx!3`cZMHbJ!bSRAY~dW=lASYvf|E9*zImDP3%BeQuvVCq7xC
zGL@_KxqQO8LpYjZ!C3l~2k*t~4A?gA%HHUcBCgvmUx&R>X(F-&U=`MGPUB(e>!uca
zHT*6me<19EYF;;9jxQJiZzw*OWQPB!#p<sXd->Vli?0)zGgfVdVtjo*!gH8Io@QjG
zAs^#uLzwi9D^}O31>uMb$LCVDJgso|QOml6;=u{)A#3J~=?ze+VL=$E)SQjcy>8~N
zj%G3D_pe@V9>KNV?iXwaE*y5^t5&_TA_^bVHE6@`@W{1sc}l7AA8*@?=l9q0^J*;g
z;@Qre9oMTXLh+-@+%=HF_6BtRZ0D)!!g?NAcw$*juDe8(qL>>4rv&p{hrPDHLoZUG
zlJEIm?K-2MWw8;QenpzJhUJv}fcPfJ5BPdDpq#_mjX)h1tD{Zv0)*F){}(ZM-9Vih
zPs<-T{qpU@nubv9LskpyW20gp)#<Ztm0>I|DHPF*ny;~}C79dH?it9!SkuTgu<gj9
ztYZnkp;lmJDK$E}+~43!O_av<C(SKEU98f{R_O0BF|-nfk(|I4qtO3H8K#SUq=mkL
z64RBnk<gvQ%s3iF6-Jp2kR1TCf!!*Wr_6KMN>Y~oO$%KQ1sAbC;L?TlwN-{eji5N{
zufMX;1ARwb{gqd<;?0J6oPJv;>>COU-&WCp*{!kPa3#sG2;UH5UqkNwnrOe2S+@?e
z6N&(=EL<+ltX?HW{*B+%Q@8n7(thb8%V7FOrR+<y%9w(UutHPkp*p4~HaQf=*Lz@S
z<28!E$PNZ>&?34uzFtC8f9Ws8PC+66`o{3CsM1<iBmW}5Z!6G@9LoIVAN`l|E8z9j
zf3fmYUBn#p7t?-G>_c|e>r@T0s%4@57G3re{to>XfeIb1v|p4gUztM$vh!co_o!Ie
zuNIv<N(>{}FT^lG9csE(2S2K!6bjOM^*SkDK-u%KP_dYL&x84`;Rbew7gxu?x{8WG
zP1X|D=hXPUYL?c7&q59P_ACE4&M7?7Di_i?g`s9b__;6m2_|)?FzrQ4bNvIb^y|VK
zhu?qX(-{^Esu>El26FtxO|Mv8M)nHw3Wk_{Dz?gNH4nc*3!$dAP<es@p6<Vdr)N;S
zCt1VBogY(HkFXwk=w2!w>bagqk<c$zvu5nChVFpnwHX}J;nRCqh|(HGvmzva)Ae(p
zDES-V@{H%Wtp0SaxkX%`zFmIjc}_rvlifzu$8uG7unn;HhDT&=NiaCmC1JRoL}Z+G
XZYS|R;ISdkR~)s~CMXwmtNQvsX_c68

literal 37388
zcmeHQ2|QGL`yWe^>~%?TldWuxqEckZPHIw;sHw=3ETxbXp(!mYgi=u#HxynJ|B@wI
zL`7G1vt&ylvLwbf=YP&H?li`5XWp4Py_}C`=KSW&`RMC;zR&V|o@dOA^!3?85C{vs
zb*QZ<)ook|1cDX(7r};@gFt9+Ff!!;&xQ}}4W|06EQGV+E40AN!Ow}efXf%T5Z77c
zBH895mYS}NGQBA!CB=?dZ(`!f5}>ZEmCe%6BH6;>ZCz0R(kBOXMy#VF<nuxyr^c>8
zRQsvG2qkPf8hJv)wlltL*_a=t4g_vU^$}a#^*p&|$r*iE>5c{KR0M?g$lAMcJuu8a
zeteb>N9lQXy}!>`R<S0BD50bM(JUE~+5#RD;hu=3*mJ^PY9sQTIZJhSoQ7`lH~5QF
ze?UlZksu)s-VQ>d3w#b6MMA?_CDh2D9f9~eJp3CQhnL+6FSc%0gL1^uLt%%b-Pl+{
z{ED<}W0s>tmT-r07_&)_EGQ%`U9jTjtGdhqrH`_PuRM8jKVUkI_1&|!ArQC$t19z%
z4s2y7xoq<l&hvdk&wak5BceSp*GGDw4p}qnIM$^1F*-+ZXwr)V_4}h)`Kbp4loV5a
zP7*|*`$33wkRT!xYIFjWu`!5*?z?Eb=5-c}Tal{mwdET`7ITMUj0&XcLhuV9BD2Ru
zq`#;AZR{JQ#s@XC`N&#F+#Z$uBj?pmvh3Ba`z^9cVf7M+z#HrQ(qcN!l(-0qhN-OE
zxJb(q<)vr0YBsO&{6&i|#wgrYmq=f5@{}|Wf9P|R(3an4=g|PizruqK64D>-+H2Gw
z-IN=kcu&nl|5?ul;n#zD?$Ux5)rMSVVR5p?|N6nek5)W*$B~a7@Wyo3n0^=JVDRGw
zsf|Nc>%}(dRwtyYuJ%g#L$nO7*igQ;YOoH~QY=&Nj<&ff-<a)vrZW1PoS3C;)dr_l
zcRioX3!ALmzn|1eQiuwAc`Vo=CA_mm-sQO4?ZcLa+q`SVsmBG<8t$>E;2E_Wye<E1
z0;6g;ue@p$D=BeP+z{c%6XfVpt7MOH5cbp5k~*fv`enOCy|3;;H6yXu;5P=2k{OpO
z4BMnULPi#N2M)f!mCSF;|JKauewkZ<I3_W4$NjT`!Z8S3|7us<(pCczb<OI<Ek5S*
zL60|AZgP6rtS-o7MCHqT>Hz^iIT4eTPsFVi4hV#G^OCO`wH%{#O2V&}Tdh|*>0B>x
z?b)4ecIn!;1-|J#^<n3}xTS*Z+SlfayCuP8v>UiPdmXD#{<~7SQwP7gb(`s*FJ%+S
z?2bQ@8>jqoNcM(n^ZM_;#`aPDo7HRHT=|R(d%cSMn?<{5K;+-xv7DT(p&5_Dc0lTa
z#j;Z0Pqr(YR_|?ksoEWC*=eF7YyU(68y7t{y*}K>?Dd&RFR9UDXz=nuYI_Nm2C}{M
zmyf`9;FMTj%U~|{zAVq(-?wJ<t_wUS_x;<pnCpgiYD2dAyGlI!pP$Q*Z2xgdBw$(1
zR|#bfo7{K0cHGZ=@0>0X2?i_$B9+B(X_89Am%k|OctWFukl|K5i$L6pB4&{*<FQD*
zA7aL<jFdv$=})7?LBb?rJ&h@P*5c0>CC0vYzE#Je&0{X}&{>!hsRe3{3=+sv)<{l>
zhLp!-`2#GsF0T}>4y|)mJwo8chR7~!1KBk;MF&J<n=eQg9t@42Xn8(oHt07|eL#9c
zNlr2*!y}u7ToAiR$m1VxjrB!ZUHW#>f9RufDAILqti9SvKLif@9SL-3I~&A{dif4)
zX1^4<FgjIG{UPRZgQrVg!=1LJb#Y$Rm8q&<)l9M+cdqhi<z4gY&A0g$=c^yG1*d0_
zyU=hCa@1H*feX_S!MFmQyoXkoHUhc0<U%a!aO=g(1?TRca4A)+nDJ06(?O_>wK~Qs
zW46cSm(r*JV)P?SM?|q5M8u=LcIy*O;QC#RxX0n3^#%P6hB?)dIb3O>b1?C{hYrM|
zT}se1ERBK+HY?$>-XJsWkp6dK3+oaheXSjZE7OJZx0>y7D=*PZRyo`D;f&wte~*e6
z5F%te61*P|c_iHvYYkr>0zre`^tPjc&WD-H+gyBc)&8%nnp|^#Aio%SR~(AC`zMu8
z{Cc(GRDdz^5eO5(QN<gMdqInX=huGlnv2BzI<A)%?>C4*%n<Z!3LfjA5qQ?EJyX$J
z?jK@Z&bcpPMWsWQwm0aP^aj-cU7OJjF57c~+T%M^ZI%h8ZCx6hZ&e6*yI*KYRn+pu
zY-qm6f)i1TFQ19E{T78<SQq-hnbPw4MZ03C2L^A8nJg`uASkb+3+#_;0-fC5-Vv5p
z>~i?#c{=X*kPluOv8*J;w_5B>!`ZkJclnxLgFRXG&F-QvdWXvXPF8(j{cQOo;1KDB
z2PF2N_@y!Fxm5#vS6T1AuqYb9V*V68W{?_jBQ;LS?%kHtOO_^llgQ0_iZRo7-fQUZ
z-*Tf~e&C$LUdnIUNPU2kLR23U@htsWrDqX{NsvuKb#*i_=u)*i2jDraCg6PAqA&5I
zW$?RWf~AL+`#PGtDm-)22y$2>qM4DH8E$hgS~o+CJG#Q$s(4w&>s#A-F&o?D?lwlY
z*_SEYQD3G-H6VD+;TX#)c%J<R-j@D|{CEZ;^4E&bd|wLyoUzlObJ4@AnH#WHE$*_8
zatF5?UkeZUQ#v-GXp$#(VCY2kF^<(4je{A)us>GS08K_%sX@WxcD_LI+Rv+w`fqfz
zt(+y_@^}-*G&LmWj`X~LaVnNaH6p`tFeU$${u0t%iiaag-87mr1?aPXU6&qTgmHZK
z>7K;CODhbo#+sVRB+S_ol&Jf5k_)0y;#$;00!ayTb*5Xs254UC1tB7J0qDCb^68Hv
zmt&ksZ~8AcA2aUGiStIAJwhhzj3`erX|ag=Ay4^@HPmA<ah`<!^6~vx4bZ~f$Key8
zX^sTZkXe19;h^dDlQHP%#IJU*0zHMClv*BqNWUre<Z8*9<oo-Z3=Y_(^CsPmS|d<p
zd8dH%qWuybG-L)K>0lp+QCmTv8Gr|iCeTva=irl}>D~?u)^BU^^!TRAr)ZPWnwPFu
zyMA$-$jpYWqd5lQ|EciUWqGSZW=_w!fX=?PHIl{K+SHU3%Dm>x8qn9q4&G?LR9{Pj
za4;W>E>&OPtKVRT#`cAebpV{VU3UJE*?)woJ7|!KVLc(^qcYmIbS421AhnvRe9Zv>
zuvU5JE<mVNd6j9!=L4rjY}1}P%$UETq=A2V(cW{VTq;gio{H}cDHe&D>+oLroK~@P
zxU$xgvSj;tC&En>?EFlO(rC?%P|_6@qKJm<?nH$HUmlHdUMF8WFfenyi2I;LF3@D4
z$HONnbO$%Pqs=pP=k|7)?0X+gk2qyt?RtM?MPqW8Q}K>Z@53J!r6`VlTZ;xw8I})H
zt0E5G5!$(eJTXRK(E+54VDSPN#^ON5-C8cLCmI5=rv-Sj?AR^%!&H*)&EoxfB&piP
zTE|7?#BYVtJaTR8bRK-%*J~BQUy0eJcVL!|alFiPTGVAC8_N(hULb)1IqdoiLj$=$
zv&z2c3%7CaY=M-ce|+YT`=s@kj~U0&#l>~rnUBi8bDDLDJLbr;tqpDLcx3glsV`9W
z2)|0r+!Hs7y0AO%)YBBok@eC+*}!uLBpanfdNTh03>5j(82fkJ0wFP3#Pt;yfCjp+
zuGskO!`@Rn_nQS-3Z_Zh8b7~$NnT>B?wtuwQ^DV!>I0Gu1c`-(?gwc~6Zp&!*|2H{
zdP-m2HW}It04=k1(8$6s{MRLPRtVQ`xq0;WKR2qF9|(S7mUPa;?6S2@T3BL(SXi9J
zyw*6mtY>>=&7Z2t8YeINV7%oy)hG-z2GeoSb{)YX3&O$Y%9mFxcYUqizLj~#*7uOt
zu61=(>SvdND-*oT(V`ItQsz_w??+(HLqys^#)`1@;x_*m>_~gFCeX_<An^s~y%6|e
z|JOQX+wV{Lop}|Dq_yXjiCTpi`!BntnkjXe-A4C-i;*1X)6?};AzK8_3q1KhsoRqC
zCC?r-Lg}j*2z?51rvVH+SCCdfphnQ>0G}CzMjQb2&SVwk#=X-VL1SKd<>h(U0E|K&
z#<=Z9VD`>gUfF`3Uefg)CQoW3&gLV9{dXeS$1cy9iCIDeAP`%S`dz6k!`zAA+vc8l
zZ(Gy1aqWMBJ=^`!K(Fcy!~KAmFYqI`-=D3!;UdS9%KZ+BH_g{2o;2r5{B4WeZFAIw
zr?MAQp}#4$2ZRZ!j1V+ts>%o{gNTFRPF)6>wkt5PuQ2g5Nb4>6N3;W~w`7RF1qa6y
zZ(B)op&(D-{cxz}gK>U1TtX9A#A&dKTBq|Fpdx?=IUj?-8jz1c|2!zHQeAjFD9Hx>
z@qkw@l$1w7*%;+f2JN$bhfSg!2HFC@T@+G}oWLH8*_Vdf9vc~b!N(_%-$8%wfa4x4
zX;e>O5Gbl=5QD&Or=Gbw1GicZ=IRU_YkkP*44y?8Q&S1giZJG4(hJMV!Rb0v(1?sV
z$mNm=UI}9~2%c5Rlz28{;!16r>a?-2tX*d+BGdg1U<=)^-2+ojq&4*Erh{Xj)TFz|
zD0DZ_);$Kto;uQ<I)cUN0PPd~<)jdHU6+%^gUnt(s{zejsJ~WMZWs}dki~<x2@tUC
z+9tyy_!wupzkk@F{kd}tUjso@Av1pc7;XLiaGV4|iAjhxXR1sJA@y3tL4kp4AfOy0
zF&=ba5}3}C!40bn_&BUJ7r}C82$9d=ipnUr;^z$cA=nEuGTr7W7pTn6TNop#5n~@$
zqM`7<xRLW_4d1M#yF2Z2&`b1qv0g^*d}mj5Vds3_INy2B*dIQy$r3(8hM<^tS}sP#
z0DLh{7GSiJpP!gbqsE(aKpUG*&l&n@64msap`Ryn!FbNlWNj`TYz!>DRZ4i2k6M^<
z=C}P1Ml)ox@uSR&B4$>)O^|_K#7k;_2^j#$FQdPVz%eH>SwyBz?u-&58I$1=8fK+T
z&lrM+w*8cGhGh<(pQLLF<Pki#VnH7HiCZb=S813X1Ha9h@w|Bk?ux^6PmfX-t$SiV
zp|L9$hC_znAStk{6`;f_464I|U115itO>E#Mt=>0WA)K=bs8a$1jz@b$dCT=0jttQ
zJhxKvVCjzs94lOzt1ob?fF|RS;JsI9Uyng{23|$-c&%Y>PYHZlC?G~<E*9|WuVJor
zfnV<x89eap0aekI66X~u#J)7oPl3s>AxKw9ZHqXF1EJrYAsxN2>e?hlgW#((Z1NIZ
z8hY_Ig|yq7ARAM!+necg5Ma{(&vYDMGG~Z%cS>m_gNm=BV&Ehhm<$`jvjwSTMU7Yz
z$g)nwX+xxVfYXNXmX(2cz-7=7S@{5UhS7MyY8n-(uR*Y`kgvfYeSrymjN^ym{c^_6
zHH3kA!Rrds?JP|E4N?!3e}prpo(D>aEf^A~!74H#MS~z042#oX6}6Ij8Xy`|I;TMf
zk5Ny9{yPZp%Xv-bLqW3v9?Z`T!z)kDTs?u;CJH1Qq)|PAN1>>mK|4!u+HAw?%k*w6
zo;pSzASq$4*1&JSo{}#{e``BEYlyhs6pqzC({*9Q3^M$B9BYI3QW-Qs29`C1WNJI`
zm_W51^yiZ4YE!}&j}vnWJnNDv`6U?>*CqcC&Kjb`v#3c{Hqi8quvpkNJ51=o(p^%1
zxo_PrC(@niuBoZpSw|_#p}&m4ulJHtZea54`YI!(5O?~MM~Iq{SZwec(y!hY>uF5U
zvlf58C^7cE^R0ircpIsO!1YZ}9QuIiG}d>|+6E5BfMpLiCGRzih*#;+)9!*C41T;I
zwQ<O5z1Sw*>V#C))m|xoh?b!h8_Ks<4c4Jrie>8E(Kc7*8?(L7R7PKu6SK6f+Thgc
zuIH0^VUv~n_metF3Q<8Xj|DrVgm<>cyBv4Beb~})n|H1F*!X3#D--2Q=#9$sw4tBp
kW597<2&vY9;x<~(&p#=@m8KagzsSTP6gUoOYE2yaf0h}wZU6uP

diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt
index 070bd8c1..482023e3 100644
--- a/panther_lights/CMakeLists.txt
+++ b/panther_lights/CMakeLists.txt
@@ -85,7 +85,8 @@ install(
 
 install(TARGETS panther_animation_plugins LIBRARY DESTINATION lib)
 
-install(DIRECTORY animations config launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY animations config launch test
+        DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
diff --git a/panther_lights/CONFIGURATION.md b/panther_lights/CONFIGURATION.md
index 75f1d434..2c6d6421 100644
--- a/panther_lights/CONFIGURATION.md
+++ b/panther_lights/CONFIGURATION.md
@@ -2,7 +2,7 @@
 
 ## LED Animations
 
-Basic led configuration is loaded from [`led_config.yaml`](config/led_config.yaml) file. It includes definition of robot panels, virtual segments and default animations. Default animations can be found in the table below:
+Basic led configuration is loaded from [`{robot_model}_animations.yaml`](config) file. It includes definition of robot panels, virtual segments and default animations. The default appearance of the animation when looking at the robot from the front or back is as follows:
 
 |  ID   | NAME              | PRIORITY | ANIMATION                                          |
 | :---: | ----------------- | :------: | -------------------------------------------------- |
@@ -121,7 +121,7 @@ user_animations:
       - type: panther_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find my_custom_animation_package)/animations/custom_image.png
+          image: $(find custom_pkg)/animations/custom_image.png
           duration: 3
           repeat: 1
 
@@ -133,22 +133,21 @@ user_animations:
       - type: panther_lights::ImageAnimation
         segments: front
         animation:
-          image: $(find panther_lights)/animations/triangle01_blue.png
+          image: $(find custom_pkg)/animations/front_custom_image.png
           duration: 2
           repeat: 2
       - type: panther_lights::ImageAnimation
         segments: rear
         animation:
-          image: $(find panther_lights)/animations/triangle01_red.png
+          image: $(find custom_pkg)/animations/rear_custom_image.png
           duration: 3
           repeat: 1
 ```
 
-> [!NOTE]
-> ID numbers from 0 to 19 are reserved for system animations.
-
-> [!NOTE]
-> Priority **1** is reserved for crucial system animations. Users can only define animations with priority **2** and **3**.
+> [!IMPORTANT]
+>
+> - ID numbers from 0 to 19 are reserved for system animations.
+> - Priority **1** is reserved for crucial system animations. Users can only define animations with priority **2** and **3**.
 
 Remember to modify launch command to use user animations:
 
diff --git a/panther_lights/README.md b/panther_lights/README.md
index 44b288c1..548b0403 100644
--- a/panther_lights/README.md
+++ b/panther_lights/README.md
@@ -10,7 +10,8 @@ This package contains:
 
 ## Configuration Files
 
-- [`led_config.yaml`](./config/led_config.yaml): Defines and describes the appearance and parameters of the animations.
+- [`{robot_model}_animations.yaml`](./config): Defines and describes the appearance and parameters of the animations for specific robot.
+- [`{robot_model}_driver.yaml`](./config): Defines and describes specific hardware configuration for specific robot.
 
 ## ROS Nodes
 
@@ -29,7 +30,7 @@ This node is of type rclcpp_components is responsible for processing animations
 
 #### Parameters
 
-- `~animations_config_path` [*string*, default: **$(find panther_lights)/panther_lights/config/led_config.yaml**]: Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations.
+- `~animations_config_path` [*string*, default: **$(find panther_lights)/panther_lights/config/{robot_model}_animations.yaml**]: Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations.
 - `~controller_frequency` [*float*, default: **50.0**]: Frequency [Hz] at which the lights controller node will process animations.
 - `~user_led_animations_path` [*string*, default: **None**]: Path to a YAML file with a description of the user defined animations.
 
diff --git a/panther_lights/animations/lynx/battery_critical.png b/panther_lights/animations/lynx/battery_critical.png
new file mode 100644
index 0000000000000000000000000000000000000000..e5fc772315dc4e215ea31fa054f496d76b9ee547
GIT binary patch
literal 7229
zcmeHKXIN8Nw+>wqLAsQI7^Db>kOCp0_l`70iX{~zq)0;Vs7ORW={5uuL3$AcYylD#
zL=hFRFp3DMC<Jj(!1f)YGo$x@f9AP&{&}9~l(qL-?^^3!>+G`++so5wwWO*f2n1T~
z;%x5&+zo-VZG|{+J(2c=0s={-#rkb!`QW0!jBq-c5=H{EVi+VaDVjnCfubLk7gBS^
z^`#xY#wj+7TBm1EfQ4k~67)r8!^!!MP<?z^$eZ5t@`QD-wr}$uUiw&{@Hs<k|B1Sq
zmi+Cmqc<!*pxP+UFxNi+W_RT(U#s>*z^8;yP5o}{b<gzs`?ow<JoR>gG}<mT^M|tC
zt)~+2KkhBw@p1ZiY1BH5hBj?7wDM}&dJCDc(4FSE{da>t8;i!-#XU{B%Xxv_@m^&x
zYVN|*_MFS-YP2>$*|ulx$0Mw7>+V3by^7cE+@RAExBiLN{QSOspYkf|Pd^{9tee!-
z{ha11ZFPFS{o<9=`S;8mu4uQ%Z%b0YTK45o@5!Yd7Q6Y|mJ;F@2vd?zo02T!`dj%$
zZ2Y)w)lIegx_KY7Vy?-Byo$ut@9TAfd-I!_CR?YyaN;u8UNv494K|xJeV%Ss_i<`q
z54<9xyw}6Ecw8gQ$9GfW^MZbrsHzgI4U+w|A;e|J6>i)^jZYo^p*7}OzP{Y8<M~#W
z^;QWuzON)a4ZXN{4PBlaHe!0?B>n8$^)u*mvAPS)L@VjKRBej@OYOTZH5NMV&#hd+
z=sR8Z6H7*jp+!}WO2L*BY+>ONB?kh#^sFCY{gKtl;&r~(J=Wl=v+Udr843-QR~v5`
z7(emN)k%Y0lSLl(+Thcf)*(`JLh;M}k<mL|12t6+)h5%}gL%Q{^L%c~^1RC@t9L_N
zhPK50<`GDH<XT<Z^2Bk|wlyBp<-<>6`Gkf^&fMs9=RWwxqgzcb*KBPe|DI;2*<ZK)
zQtZ1K!r2Y-pNmLC56PzpJhA*O+dF!zs%x)YOWPe(ck;qa(`(vA{@eY5@i*UpT$HHf
zDYvk+B6Nme_t)0W6CfIxfxRlp0}hb%VG*Hyo)XP>@V8%qql{Ml#*_CjJtxtuB7+AP
z`MY;Km$+zPk*+cR1vXN5EU6p%x&FpBi4H6K8LK@XSGL4FI{Y-<Vsmtk>b|Mxi@UXq
zb^Q7n4PyAChXbIus(H2Emv?KDqWn`g$_2Fw6LZh~*7Bx&zAYoR3Pbg%!Z#|M?jT<n
z!;9H2-6m{EJd%1WEFuE&$Yk+WgzGu3W!y^LwGCU1V~Sn0E{ebFK=omcZ~x@z{rFI>
zdfK+V^QMI=M{oDsToIvH#-1H|f^4A?9A#!7M)Trv)u+>j`&!&)H&=uViG&Q3a;zz>
zrUiwHZ&hJnwMM9Fh^VTtimzKqM&t$uSW}s!^d;^qyLZ!bplnH)lb);EjlFM<W%d=6
zIjfP|YrR);kH==a8p50hBj2o7PP~ned04f^q!wWvm)+8kki!+P=q#q)^vJkwJoxr$
z?-X^&jauVAx%X7fkn`aO4Y>z`$>*=U{o{S?gI)VVvhTI<^EM4G<V}SJBZeCSzy;i<
z+6|??>RwM>ZyD5Ff#wF9-U%6wWE?B#GVES&W~~f95|cUeF8NL)F)`=4vGShx@b<tQ
z%gL0rxltRRD2Lr?+^2f_LZ|jfRN<<x$b%yhgLM%;SsU)@%Ix)*ugCQn53r41iScZ!
z*&L46_(l@6%dtnfGN3r2R@G$kU`qGQ5vt0q#Jo2e*%K~<wo-W+MZagn&=@QBmv&t%
zo$08YJ{ZWgjHT_dDc>VHVI#7t?D*}P^332uzp8|r8~YM2nH&tiKAMPoXdTkCFA}F6
zdI#=1=XLZCub7?3Ci^Bz;j-hc(AwsT>dHRrlC%$@1>R(t{iGYEM`FW|cVTV$!qWqq
zDG3PY0=4PvR03gaNLx$M?Ae>TL@Ix`x<^uNN1o?ki<VTLr=zRtJlyqO!{`q42wJ=N
zCHwXgi7urNH`E4Y3@$ofPX;?p?c3#5eW4Xe)AzwZBa5Bam2J8_Zv$;<uM0fFH@^wb
z4XlM^$4<+cNV(PLo#dAKurg!xGrFcKGS*_TXRYmQZ>lWG9I3q-lVOh?Kk(TZ?|o@b
zMCh$D803Fkp_f5#>>Fa3?4C-DF(ChOtnHlc`L)xYXG7JEcTISnsB`2kp0sP7&8K#k
zync475T(BB1{fi?!Uw&EVH$)zsMF+fHgUm9jQ62S^QE_*TIxgfGg#9yNbm8`yQ61<
z!HvJav|xMYTqeFN40T=`*R$uV^_78+;u7`C`%`PvRUbiUj@!?5tx9yVNWDr~k^8{0
z^wMKFR7J?qoh&_ZpDf|}gxC}uDQqdoCbG1myXK~!PSv5N_N1-Zfme5?n2YN`Rj2nK
z2|93BWyS3yWt20-ko%G6tEU6duS?_1SmzA1=H*_D2W4OC#zpp4irDhT?vJG2;+JVB
zrN=v@$D$?Gj*CLNHfKS^MN20V?0qG8S)%I-_0;2Xcggp@@!p+xJcUvb>R&peA=y-t
z<UM=prSyJbeDeC!o18ryhh8XDhpQI458lhF#5ED$tczD1Eib=E5YLo7@l1XCL|Wn9
z)u%l4O<VXGrIN}Pg{}_SS%;g-ORJ7ttFrIYc~8r><xluOHaaex<ER1|uM4wwj5al^
z_en^SQ+i@RO~~AD6M10K`c~5?ty2fkFJl9f@aP=QR7~fqgyHTBGW;)(H=BISPyDR5
zvtGDo(`V!&2qct7v9t4Xv9tU0iwt~+Pb3_%aPGEN=k4$<*Uy($`ovjh7tsXPhj<3=
zhxqBW*j7vH6eh0lP|Z<LcIs>y9leep?BoUbOZeNY`2^Y%$?#zvk^5Sx5Xg;YjATcB
z(1_xLO1I#v9-2s29wVI@gTr&T$%@p!w1?RiIbP+uCzu>{=P=`c@4h~KQLlf_^23Xl
zTf%ij;vsLP?yb&<J{cW;?=FTfe;~lyC%vETB-H_at2In2d}^w!t`tpv;9N18-p$3A
z?w?R1JJ@L#gR}Ii92ae`k@mk+3Tg4fWyZ}Zi!&e1$qiGaAz5M@AuDAO`VXvEDwUnz
zeWu8HAhEC8YIGrYJg28c$T2G>uhcgD(`eG{#UP}W<)n@n!TMtcHAzY+Wn0PimT5lW
zaos7<myl7{(50KkJEwC0*zKF4E>ub#9qTyP6UUj5zVqRXTiUpD;xSuGnM<bz(4>^&
zppwwR>DtEZdlF!eZQ8Wty>>g>tRIGI@Mm^SPd`Owen|wiqr<yrW%pzOd#BG7V2`xb
z!yQYYQw?!MI-X=0O=SRkC=kfpDw=^KgpycbJSl`ivw-j~UWI@uL<@+&v4@cd!;Z9z
z;v5@J@{RTMBgBRhFhq!zrKEW@7671<SU7MrHH^l@Mq5AxxLDwMc^C!(3shL47LctT
zUSK<VI0=k0L>U@E9il0b2#BR5*gTv_#`@Sheun@g3&<`Oi-Co~qN1V<qmYL5@DLar
zgTcUz5HJJ+3TQx?F*Ft~8cJhoEkk_6uqQDI;S>goLZ^Y3F>!c$1j_;f0p`Jf#z$p%
zc>D!VV}54=-~$$oW5D2sMldQB_M-)p<q!#gd=KcqS}^^9ttQNe#H2@r6G#q`BpOTW
zM+hR}FMCErc$gp^A^}DUBT)fWCg2tRlS?NT53j#0mMI9KP#FR%fb5?%Srqc$Wc?J|
z@{Ayz9|Hl*|HA!A`_J43%7B)K2iBfWh*%EK#ohw4>>o>{6DUNi;E_Zi!wE<f97@KZ
z2~ZT=m;}Y32uLW2fWyH_C_I6P#Qgx}LSwRUGy-WE3II2x05~L~2@y@in?MN&1PO{F
zn-HKlV=@^^MiVh`I0}b0#^8T|*c46yx)K-mV^qsfL;wn~!{CfCa43;%gaD!;!l9;c
zG6IT2APFcGfe1G?HWfe-30OyZI28w!lS0LXkYEg2h+txwaIB4&iv<K>X!Q4pR~U{(
z1`I49?i5-?^xqSH6e`J=g<ED5jz*y2Mred78iPV2O_6^qZ6Sp-flge;gc}(mzu_*I
z1q&nt5Q|&xQvg6<0px<U3n$@N^l(2qJ<I~K91?ihQqbOD^KV6QrZ54GnB|WD-t)es
zJ>R5nuRs_@Fa-t++7^o=d~?FYMUsdDM}Xb8DZ(xsErbNT-`^YR&vD9sDVDJj-UM%i
z1FB3$z@aD%5d$^FlhIIP6r7A9VQ{8s#Q#8N(#fnSTsX-l1jrP~252BbHefvgl^cGJ
zC2ALGnI|Iz3TlLbB9MM?G}g!ni^LchA%F)6?B7ojO@<T67$gpgBqE4V6dDPK;*mf+
z7&0D5Cg71cG6wT=(EryIO|eFBECT&=isrE8h3n6vn#2BA*}p0Lu+jn4d>aE+P+&oZ
z{k0;0=WMxC{>9^ayZwtE0O(&%{t>^w==w$1KVsk?DgWxOUv&K=2L6%qukQMPqf7Gd
ze>xHk*yu$8e{6Ys^gjT9d*XOECwswvA866;ViNGQlHnY{1c9Wv%coGjfwUShD8X{^
zaFBSpQhAM#T8smU2Lg$nbg{Sbi+(hH(u^+ey~Z&c=IQR-<t8j<<Ir=e<XMQXH*nkq
zr$kG3Y3&o$J$qwb|9Hah*JD4ZQJ?`7^~E<YFv#~0w)ZD1suX_Y2+fK{OPY&lfQE%6
z`szj!#ZHJMr9^R<qhhnF3zG5Dmv%VG6k-}ct@WJSs<f+VRw4yoV4OYN1ab;L?!mTE
zC5GCAIZkuI*C3&<$vZfjqxUsIb}1qwvm6PIUDBF&S?3Emva=csE7X@n)+)r$yp9>R
z5)Klg)W`Q8;K(GcsrVwhRy6+l-W~5a@5I-^?CQT33+aC8aBrgz7Z$Bb0+r$o6HeBP
zS!hUJ^6Z;-hG)kIshhuT<o1bcy7g_Gh4bEMzlC$%#WjQaYPi=GkgMX|;klhmuK9-s
z44$m3p*6YGl!%S9L`7#6g}!Wd%kFx6n0@`=!zd>g@lCSL{aHDp8U~X(Sl2CMrTF(2
zQ@I=_?+xL;u5KNeEZ3G=G%%`<*U`0XE%F|9#XIWOLCC1Kg91TE8&EN7J67b)HejoW
z)vXr@1MKQ_71269GGlBvUVLvgTOjz2ZNR^8X*npqNwJw<yV+;#9R7WKi>q+9A~QYf
z?#g2Q$+KpzjIl;~VOYoM^Qjf-7;dGEtGQF3N9Gcku^ArJna`7Q4fgJHWJJJ&<o%VJ
z6@vo=f|<O(VzbgnYEdS;I^*GrIzuwFP3)VBFwLRw5f_>su!?5WC)uV5SCjWwZWb*D
z(!H?dBzxJ(P?fy-<~|R4ZD6M_SH1f5eI0;|wZD~v`eQB<4`undnol%05e_X$0_@pk
zEy=YR!-Yn!*uXQmwd{c8AbEMt^jd{x=aFMY+6*_maGS+m;PP6WC(2ObZRFIlSB|!>
zJ{WK)J`@->3y#lzl2gPRKd(bx_oCoxpsYLxT3Jl9+2#(48qR1pTCbBFCu%;oiE77F
zj}0AM2-f!NYi!+;DYENz@2vOZK;zc#JA6VFO92#>J(=598L5!GNZ6U(aKX(FE6Mq!
z(|t;+M0xfw@{V5SlCb$7xqMf=60E?kB%gao*k2C00-&#w`(0@ByU&#p_m^*$58m82
z${iF4$Vxv2o|Le^f`Hu;uHrYL+0T#nMiih~HkcZkk4jc(-7-*K)&x}3e_F5BxNgKd
zuruGx7238`$#)NqxF3~jknwP<{uJIMwK^j}=mLaH82c^+=__$-#4dozD@SsQTG&Br
z1f|ROX@ro)Mh+H<0sI=UgJheROXqWLtDHQiR_b6t(DMp$ZjBUpuy%83rU6i_&=DV}
z4V>DDJV0kkuiU2*G3CY%Qk@rDIBEkII^h5mDK;<>y`C&|1*IeK1)AZO6oF>UW|bB|
z@koeH^2Z(?9iRBFCuog4CjoS2p>A&2IYFdvhcG=}(+ZVi!ug~)+asfCKoRNF=$^W8
z;0tMGyyljI<W&qr(!Bo|Z%))QWz(qq!cVv88xvOH#}U*(zVgKK(&pmeX<ui%Gxa~n
CYL9jR

literal 0
HcmV?d00001

diff --git a/panther_lights/animations/lynx/battery_low.png b/panther_lights/animations/lynx/battery_low.png
new file mode 100644
index 0000000000000000000000000000000000000000..4507920c6c17a43b0576fbbfb03c68ccd6a49962
GIT binary patch
literal 7614
zcmeHKc{r5)*B{v;k-b!!rYzBz)iN0S8nQ-qvoe^)%wWh;JQPBarIJ!<Q6wUgEmUfd
zRF-6^tPv`Sr1u{6JWpNk@1N&--`{`FT-QDKxzG2U&pGFF&i%c<cap<i%k`oPq971x
zy|tB@6L8l6uBx>{z<*xi00jhEn;Pk|pY4PT2Q!#-G9{P<W=AkcU=oKy27x$zg*mtk
z^@8Gr@8!w6`8Jr8ZZUGm+Dcqp40xHTmwDAK)OTvr?N@gEPdhP|TR+#YMBFpvj09t3
zDqIf~A9tH59Ba&x^Sv<_b!mTo$5__FSC6F_18#@iN#}cxK8Bvr>EHA0eWmY;$J3V{
zm{DtC+!^`Eh>pP++fP1L@6(XJ%UH))`<;uS+iO)e-YCUSx2=fb>P!y|e&56YeK>~O
z;T|oqGFP*)mqpsz8et=aRD#T`fA|e%Eaa<u*1bn2zK^Ah^*1QUI5zm<)BNPrcz<!W
zuW+<!Ve??;Qq(%bPPIj>S<KGrn60tcZ5b<Rk204G!=DWdepvX3wLCCAZuxoUJ7%C=
z8s&J@$vQMzcwS^=@qSgSRQQBkj|Zt;)FkyybofAOk<W>^2eK+NS&@$#LY=JhEp#b{
z53=MucSi^ZNU_@0AG6AmM)JGkJ*#KfWrvF5$=UM_K09Nlm9URLSKTZ3Adu|E+LBtO
zZZFU4El;l>^HRGgGuR4~sKX2r%TrVOzY#aOJ&jP=cUB;Fz|dFT4X0e|UMDlLGpz1X
z?x~dv0v|w7F_*L($+n|2B0XX@>Zh&OtT~r$=4{t_?D7dKBLBI@7uy~3!N*+6%Uiw7
z)Ov13<&4VNbUe9!vg^>tBb%(QxFAXog&*9P>}2Swpow<5btQ0L*ClVuERUiW9>qS6
zTa;bnuI@txy`Z8gg_LV;9znIK`8D8EeL=SeB4-Gtqkf}}uj2U#TzyjZRon|od7Ht5
z^HHwucP7i8yZSbjr(D?P^XhKI!p*KrC&GH~ysLZ_Hu%ON(DL3cOM7fs--LfuLH59{
zZ;LDIo;}@Mu?!9s?voG9)BI|Cyd-bDMXZ5wT;O%s_H*Yxw(PES*;Dt>n|n_C`V+45
zJbrWnA{0lud|f5FblqaYxcU3od(i!hiJmLpA@UmR*rfj0H=j2L#!v&WM{4(TG8F_r
zheY2#zL`IAUZe7J?$`0KSDj7I_~l)%zWb6Ep`llJ+f`XF*}tmIaWn1dMNSv~uveb`
z-obZbulYkx53m>~&7RyXCAq&9Rqb}Sy<!~=33p__>t~KaxX;=({avpIO~nw)r?Ej-
zOd1|eO3sALv^PvRo#s&Aa2t)nGDKq$5x4FuXFS_ZnRym|rv1~yAS;6;y;`|VWk<Ft
z=3S%)Z@!#%;o8o)$KlRp7QQ9rFKsQ0<+F-Sp>1HUS;&B%6ZWdQW1F!^X@#C+Sdn#x
zldX+~GbU)m4*d-&!O>p!J;%aaMAQ2(Kkw_4**J3i+L!ef=~|xU6h&%haWKQ^G<mPf
zx=(YWVKNktqQ;x>+P)&^4^JZ&d=j`_)(+i-ckZq4K2-5#CWTHJ=Z5Sdju!;BmUV!(
zTWjG(_IX@>QO#$0N677~PG_yrb+Ln6eLZL{W*6iN1K%msj$}N1Bwh>3^10&hM#UBu
z-PUVe?OnD;{#GPJF#gPi$+@FODT-pb_Y^L+`+eWb_~Q?6v(zQUV|6F^Tgk=ZW8z1l
z)6YyeC!(fNq@t+I^S5S7EN*W7bT3x*iMUQGyuZ?}?ao2snO@I<6DBtYaFR7J7Hm!a
z!S<s=<{78y=o<h1F1sXbN`zxe)+tqM&>v4idvvwPF_g*<@1qR}*{u<wL4-p|pN#HG
zt2*6isJ^VOc~Z@z5%xk~R~%9ZGDEpjOU2Ct2)E+{m5UB0DwHMczg1Ej>?L@Y)6^#u
zjon>{?fG<cZ9&{_G-O{>r|S8v!1bV)T1V%&2iub?ugY?CXHCKyCC7FxlD92Rx}0p{
zzhW#A&Chi{PC9exy^Eg9HWj1P09}a>``rU8Pt(f!It$vTRHFy=6>Z}tc2&6RWLvDe
zSh-j8@-qZYq)uLK(#mJp&%JUyPbD)1xu!-~ROGC-LcH8`&lODEIWF62=R?CQT$v}2
zGDDQ5z_s=nE?Z!Yl8T!`&N)}Bg(e8B-*BfwXG0N_<}mfGx!kTP?8u;hSU{xO^L5HM
zZBNk>=!a=*TCd7rBhgnbCg_(HB$HvU`)p&^TJty3xa;^X!`{ZplvOCu%y*=0oX50v
zs(vxim{m|ynL$rx$@?Z+D22JKNyds9Y1HGDYM5h|cD}Py)#4r69qAT@+a)9o<9%f3
zwh!;f*y63eG3!xsYl1ks!ZwVryrqJ+mX>anmt)~q9~^$8TGo&0t0%Sc=u$$Q*Mh<y
z!dE8j<jB^0ypNr)l~TzOza}HLy;MLh<9=4Xx0pH`nk$?q4!agt@u|*Na*7<I*MFn~
zJf0eAqYo-lh<IBNdaSP04;!8BSrj4UC7HdyZXh}L(ZnmcsgtSrnwTg4H`a`3?P`4f
z_%0+zH~}JIp8y$7gRAa9nw6i>ohz`|rr?L%IUJOE8+E0oDHQB=U-iD|qn8qus<?(S
z=Xd%W;|f#n%6%+f_TsyIibSc8RttDnJZ<bFG#%YGU%F{2yF4O$KVSWVRfvzvr#&0a
z1g%&+*`qQ)R(G!L{Mk4>8m)I~_+c-92i!BN{T=<H-*c<UZj8Ne<@(MRf@+7k=$7aP
z2>sy3>t5+Qy^0#f(?<p8D|h>QVPD^W|DJAqJr>j}X|pWp<gjw!{MgW;d_%=)CBG#7
zarjo`E!N;7W2>LDrD}QOnbCDS8u)^d0jALptChc{w5B{_s2U&tpd7ZmrOo+Xw&kV8
zo?9uwLvNmiQDdQRQ%Y|<Zi`ZQm6|<tGVYY-=bE6q8xziXx_6H;7?G`S()VZ{H)h@E
zkFRy_QO*7+f=z_!R$kV7)FV~u*>Vr;VJ6I}ihSsZTp2%bB5l&V#Jk$Fd+P@-)F^uD
zP~sCIk-4}9&bJdgv&-sUEbork=DG1QC!*~%{+JEwj-=NbQo}<*Ev`K-{YC8Fxmd*}
z?|8XehhzCUK?$<EeWX8vKzxZ5Q&R_PQ`0{WCctqdFZwvvs>x_ei?4H`T9&xna>I7h
z&@!+ZWUog8#6`8j<eIp0PRv?6g$x;a%f^bKp$GWqjV<nO!fwVJmO&w53@7$+sqZ;5
z9?cv^e|q?WV)zWGz!hKAt0j8n6zSq?1R~R0lK;lI8O$WtqQ2QSTI-~311suP(}V6B
z)s7Da3uEK1OlAHk$edW$`V`I?4zueiW=8t3yQ9;QjwDO5JK(t;-K3mB9r-PC9CDA<
z<<TQe&G>?Z5jnECsZu^TO|{5k(d0fU;iH^?g$pkAzz2CDR^JDyZi+Z0O>j3vL=vgi
zV<aMX{#M|{T&vC)Zj<5Am&~^rtrdJ0X%Qz1Oqk0<v9mRvC_{r$WkG_`*A!~37~di9
zb3PT*GlZ8VXF)6eLwkaj9&7qdW_}8EPT9g&KplF0=W6SLh7s{63m0t?-&)0-GBJ?2
zd$v=XbSU5RT+s9B;+yGR!eBdZCECGwQ@<*s-XO)9_W{$>gQ(P%7*Mq~vuRc`Bn>!1
zexm?K#r<}+x&%5^14pFeNg5m~12{^8K>CIp296L!VuSG{e+mr?nW?FVfGI>Q#7)x<
zZpSbs1yHOanIz}Py)J~vAOeO6F*Fd>=jZ|eR1zBp=1_xaEL{#3vWlw<$UHR+0$w#?
z2Vo)m?Hs_SbS4Rm)<A2(q2?S)7!qP23f5;5$+}Kv7C#|?CoCj@&1UGrVBz868sR7n
zI@2G9z+f;iI1+|LLIDdXD}u(xaiBET4j#l03^Njoz@#wP6gmye!^Gj~p=>M!0<44o
z%#X^jv-=C4#`?(uzz2+jW55s^a2S;e`_+TRHV*?pekSxEJy<Tli5uobV$nmH1d@3e
ziN@aXD+H17mp>zv8N6B!kpLqFlc<0x3y6yNEu^Kjox@)qJPQ0NRK}_oK=yBzYzp~r
zvVO~rx3XH!uZaNef8qXS{b%i~#(<Tbovs<35Xwu>+6)Wf#n&a$2^6C4sz^X1&?vYz
z5{e?Bbf9QWA_0mcqP3wYO&l6UCKJ&ZP4X{L)-)CyM<bAUPyo0F1;D}Ja3nYePk^Fy
za40Amj?jkUF&GpSM}{Lb;W}tI3Jw1S!k$S1rV<zYYgRlcA^=4o>fkiBb#PEEO*9^g
zMv{<FJV8er3M7ajlHn*M4*mm*NYJ&QGpRVBofIn0p9Evj{8wY}2-h`su*O1=8b3@|
zH4eczHW_fhLTo9tP|n|`E)*)snT_MIiO@!BBY;RcnrLk_8io1W$d$xo0h7qXM8Gvr
zKX7?%(FKYDh{f^d6ae_MN?lVX3CE^0UFh^+EQFT;nCH1V-eCP7O|hb|0E-CT#Q&Um
zXHv+Irys9CFlBWK3|<{uT^!*@5Ed?sL|hF7`2AQS1mI}?B;fu2IZ%J<DgU8aaI&VB
z77mYuYHDkdp=bgb3B@6Jgp=V!6ahy@Ajs(7(OGmdI~>O(8T$j30@VNmv|0^Vb(P9p
zzgvd~ka#@7k!UC!14W`-5ZbzMgf0T94oB(&&4c~-OGIgDYQr(wI4DvJfduA}jDz9`
zBoY*-N!B5fFhnhFEyC|f|6fbg;gyKg{Jlhd7;oeHv#I*9|JC*%2EXid05v~!zzzy*
z$gsb5<e!}JCgs2Q`#Em^#RvfOzd`;HzyH$pFJ1qLfq!KD?{xi3*FR$59~u8UUH@-%
ziT?dfN1_1-y>Q@*%`9tN7Wmo|!rNGyt$z1`7ENnn0ZD{m<<0_u#G82+UzWP~W<V*-
zwze}D9v6|{z_%&KMqmO25|p<#Gj`$hO`ox&E4axdo~`DBDoyju0-qK2qF>G1v%Aio
z+2`gRnnjW;?o+i}!gZh6*c8PM!L7v^yszcwVxDsEm;Hg3fQ@0smOxAa8Fp2^%It_(
z-2w5zSboE(b-rzWrw%58m!{?fkBybu4ln3&=7nOG1%2ud*UQb~ro()jBujMSTG!sc
zuzo60t0*Fh@2wyabFuoH2;*$cV7c^{VEzM|iSKslpEN<14TV0DagZr(_gevN>n?o+
zZd-o;3N!auAm6KpxwwSzQu-_#utyK-cQf&J`0Rr%w3%sX9d(X=93}XWf2Q~A(6@qG
zbNhsD+k6QZNy+*aoqtAT`JPd*A8|J$xF}&+Cel;QU0h<9KJl~cY%TX@b6)djk>D<x
zG^&kyhAucSSuV}V$|cw%<bz(yMYl}laeVP%LNhkpy+hH-z1X?@W-*}|Pww8Ng<WKo
zS%VfaAqgoKe?EPv=14Cz#2#_`)5|wgu20Jk2`4S~Zqk2DU8?h+ZBP1|95Iz9C8h`&
zMd<C>P<12s^0?}7(t#rpQ;ypN2KI3YN0)}0m5yu@&@i^8Gj`z<l{AcOVGL`0q>6_5
zswVP~jZ~Lm$acJLyp431dI+$@(`@oOl^c_^PO3}9>wIo|60pS6>>ZM7VcYk+ad*&*
z?>3sY=zo5prW!&=*q2s_u33=%xMgdhtiB7^Ib7y;Gyi}FCe)H5AR+m2%Y}hma~k;k
zz-M}gY-GzNy~=W@lh(<W%X!_&O--_qTh&wyJ#7iz>s6YYHKfpd-40mdX|4-&gUQja
z@3rWIx*=rJYhVe`jPg~<A+AZgfkzcNrMdn+9u>*}1(-_L;-LY2J|*b+T15!ick2jz
z4utqRuBybzxoxvFu6iKpaFlChx??P8z?{oUTf#D^_%*q`R%dfbult&JH&!Hx_-Pz9
zKs;`)NUG*{Q&v70fqtQ)eDI{2jC4b@#o1gr{ind=rUy&Sw|4jz<bp;g3co4VDkG-f
z9;NC%+ez--mm#?`xmf_7*;8qH!O9l&aq3<&#&IVA6tW&xGR&L;lkGjP^_4#g#s}TC
z<JMKWR`9DL&Wde&ODzt3Is9-cE=`6l1NzwV{L;0xuj0GC=N|jd1_Vp&cf5Gv0;oA%
z<+O7_Ct}iOws|D;beu)B2n}-BQbd|X4?hAy)HWz3O$cZ>+TLge1R)Iv+cBr(%9l`8
zXoP2DVe&*CAiQi$NS<&}1KPMIN0q#ziikKP1qi4rKZK`^c~NfhkW<Tb!JNv94<#YH
zJk@8Mj+a$1h2;kv6@t8&KsR&x9n~5O)0bW}7p5&mG6eB2vXj|}pvJ<K2vCVRx33ux
z)|QxYA31e`$x2m1h=m5tB=?g%p;0y|!a>cWd9{-wC3f8S=87Xcnn!)E21rRa$cVP|
z-w0{+ymnt9=Wa#d^^jK2Ym*9YJ&t=Y6`?YoY6*2|WkH9NExzUFodPP)A6Wv3rj*#*
zg38ZiYcE6s4eaXQbJd)?m2=4kp#B)G*=s^lx}ksfaJMH%`*5tm<ObjgI^1;6q$BHy
zh~GflaED<3Zd~;X$3f7XeSPEUdg``%?@6o8;FU$_UEiMJ3rf_3<0G;yvFhuJ0Wm=9
y{1XAMxURYSmCq|~D<Fe|5<~o7e!Eq@n>QpJRo@0`+dstHRISbTnw6ON#r+S@>sOxu

literal 0
HcmV?d00001

diff --git a/panther_lights/animations/strip01_purple.png b/panther_lights/animations/lynx/strip01_purple.png
similarity index 100%
rename from panther_lights/animations/strip01_purple.png
rename to panther_lights/animations/lynx/strip01_purple.png
diff --git a/panther_lights/animations/strip01_red.png b/panther_lights/animations/lynx/strip01_red.png
similarity index 100%
rename from panther_lights/animations/strip01_red.png
rename to panther_lights/animations/lynx/strip01_red.png
diff --git a/panther_lights/animations/lynx/triangle01_blue.png b/panther_lights/animations/lynx/triangle01_blue.png
new file mode 100644
index 0000000000000000000000000000000000000000..e1c907cb0bf8a540c88fa7ece67640387fe40df7
GIT binary patch
literal 6497
zcmeHLc|26@+n=#zB+)_}rjeo=vzayfj6JeX2+eXvgISsxW=Pp8JQY9Lo+Qyqn>H;d
zLQ#~87A=|<B@#sq9_l@#{qz3*>GQtNf6eD}=A3ii*Y&-x>wA6AeeV0DdwDuf(VC+L
zfk39Xy3l;Uvjupp($oO|^?XyuArOtTaegZ$J`5>bC=zhEQ2<;LD+J(xjLU&QWDkzg
zd3i6)bjDiEKWb{uhZ#D@UpPy=|6H(o*{RCPD$XHk<A&VTOHE2N)%}7#k0?Ap*ZB+g
zFTUIEk2}_L$Gy1i{rV5R`JxLO$`2oTWw_pG1-8gQow_xz-x<|9Tt^xmvij%w#?X)D
z>7j;NLoc2?HsCsKGZo==w>K1ZrzgJtSomu25>79F(VjTl%teR~oZWeZ-TN;M7)2N-
z<Q^?G3%o2qb}Tm>4-nyBh>PqLW*c60dv>pre;C6e)5?}EbSl^}lD@q)tjn%<7})~b
z+|V~?Ph{G$hH$%dUEMO<3`9-#2#aqc8oK;p7XXktQguE|YVYh@QOzAuRL3fe=2Uw}
zj#kTCk8{?atub5_>oF8}NnqRMOMBio(ncBvY!4vDH5Hww`XX<wbo6ty7z%8UG^bCE
z8E3y6bz2*JSM4rAa-;8DaE4WF*^WtzkM8aFOB_GomYqyEdf3oXmx(+)>w%AxN@w_S
z=mOMpguOMfk120b!)s-3q2@L()31Cq+Qs*3#&x-(ksGgS?jCEEjvjm3c`z?cR5jRc
z5#L<qNy~x_?AGAeZTy(+cYfMfWx_?BW7=w6ZsK|s6NO~OtxYyfmkn-eQ|;H$tvliu
z+WjRD($&W3Hn}a`m)7KlK`b~4Ew<|}y>dGVlAe<ecLxFzQ+g5w@hr&>eRBocFE^6O
z08|O3%tZggiM9cANi*tv9BG&LrMsK;mRb3?nY1#Z?liQw1Yysvc@&X-;CfJGXWbUu
zi{N?DJ=Y(b$Su)gd2gaU4Yg}s-|dQE!DHV{=;&4oyZiX9&6$4#?`cUZb`c&9&EBG4
zdb(%({^o$-i_DaWozllAqk|vt{>dOl4CVB-DvXPrrbcu*^vrrN7r(XO!hvFwMIy8+
zbN@65u~?(Q?oCP{-!GEfWY}`txMcA@xC>dnoRS8;R9V_|fEY7X1PdH<7P{ZHcy#Ua
zmQ|FS>!+U&Tl<e;#NeB0&mG=a)MvN1l-g8>AzPlNx*u_`nlX06JE(AP9XBX0`r@sR
zfyEq+!i|qym`Cz5_r+*0pc|bb>Q^rmZ|(NN(P}ToZyi|t=KidOkzq$vj_YPX6An43
zTx@Sk;nYyVDk|rBwV#<>|9D2x%Cp&TLg}{xD(G{bEeXu+aFRmR?MHRJbfIhJX6M<@
zQhR7;*g%bXJ(LkuPB?s{?!s^_Rduszg&j8VZHxWKPX_?qZi!-Ev^c=&oN0WEgtozV
z2)QwPxw+x}%%T#P)Di8_E3@kTEm}Bzr%(2m+{)}}+>D9-4<4Er+F19ViQh%na?V<|
zqx2LisAt@54>HOn<C$CPYh-P85{0Bo=;Sfu-bh(>n2dnL9M#4{+`UT(<~gu)4cC{!
z)n6SpG9q7p0IzDxi}t?xs5U<1YTB@tZq+?{d%@tfkV&VYjl)gdxz%XhSdQbYqD^!2
zj%8E`?wwq!=D2MuRB;!3h*j6WU5$<6KWHjt(0ilojPTYSuhK%)Zzo??eY^)gIBO7g
z{F6)N(DAo7-oL$`|7p8i!`ypnXK8@J6x#->-qa9D(UhzMDh7Ll4P%nGQa7s1JU*wy
zL}%&Zw@VJUGj}Z{jHNVpN&O2|jRmjWtrEg8{&eRgxI1relsa=>MpCZJrFC2G$G$c%
zRM*do&F5;b#H;ti0w0Cr?^nWi_So!di8xQ4J3nELUQ$n-_X@2c`qig%wqJ4WDST-b
znO$%7JYulDg}?qtNZjsbA>S$XExa(Ip7N=D$EwjiGTIIJqSvNk<Mz>(BjrKGVS}}l
zCBuG>Clu7`83Wo}IBs#dcBJ}Kw}v`Bo!FEQB*f$J+Tr2^n2wu$u?f~d?twd0Ht%f(
zbrgnu>gV68^T8GU$uqU>@ZR#C27<%novM}!v&q9L^I{TH#Tn{D(1QW!$*ynd>DOpo
z+HS=?S)%0|T{QBTtGnzwXGA0?%y~(T%h4Pd@d>%zsyf?0-R4@v)1jchB2Fh6PCrjO
zOTBV8D_ec(i8c+RENE2XfArc*hNpByu-g0IdiLud6zfjYe%cfHZrhQw9~-BK@G2=<
z7K%-;u9KpZXI8%yc}2uM{M&H57ok*jGpyl45YbHk;|QZkP6{)PcS4=&SX92L=lO@;
z7n0Up%cfiY^+aqF0GmcvbZA`c9i80zx_lL`KWVy)S?HyyI@swj|Kc+ng>Qr>GE%O`
zAUr;<YdaefN{SsB%o9BCZdBQPX6E+OUam#x4qGg}j{Ui5)oYzMJH=1ORBzpQU1Qzo
ztRgW#H5&qfZst;{UanN?w^tbW;woI9W$jXLH}7(o@BYQPI{IU^^QqA%;foQTL7NbM
zW)%*Hbr$At(DazIeTISar3$&ck$LCR<v{u*y1nigWNnPlN0K$|bN-B=2AS|trc_}h
z?T3^EFb{PQv`Pzrea|qM><~TG@@^W+;cus!hUM!CJC@gq;~&>I-m5ZeeQ%?9)g2&O
zs2Y!Wr`<MXi)@!n)OMTPKRq?j+b69x-C4U5{?6nckpF~aFi&5`x$koDWm<g$vt(13
zKF5)2S_IE9JLELta0S>js2^V8$Jn;+y@7`K!TV|VxH^ao^`!`y9(M74JDC2S(-Hgr
zcDc2orQTLPoc&_^)e5LnMr=-rgJ?{iFi;hYv$c7-P@QG>&lX;SHZ&=uIJBa#pY`zA
zZpf!_xm)CDGk$eX_E3cHmU++;p8Q#5+0}KmT{<@v``kCbaM@7cU^BIP_bnojR1{ns
zd8hB_iOjZ1aE}mEeqsuB^*OtaNTdF@Yx?@0;I@6*069+-)eq>c%>Wl0<6Ll2vC?BX
zg(cuwFxUbnU?JlP!9@oIVr456GFXv-1kMD)xqNFxe^m_v&ShI8=y(sbhmZ=a;kv|$
z0N*%IKUQ2Mi_AvY+GttHC?Eh2kTBpfUKC$Uky#@qa4F!mvKfVdPnbv|tr06dyx>%U
z2!LB!SX!Wwjxufx7Ga|Ww-T{A6d#(?R|xRV8nH$q5mHbnsZ?qq#aReM;V2B5Oh%!x
zC@dBUS|G)-d<jE_<cm#|5MMB8fS4uX3ME_tAFjk?Fa^;PYXk!9hkuKYC-m_64$l{V
zWdY;^C1VIt7z;Ft$3y+>A(lACfFNH3`mY{hKX7e^@&UwxXb}r=i~;x(lb<2jtndE9
zXi?NeI&2mShyr+^sTdp;^UILVt{z_BJ(LuLb9urEFOckCEG1md53+uVP1!S%&d-5>
z?%#2LvHq6(gfVF4;X$DZSkcPxTxr$_<@gk~fW>7~CNA-oOeTRq29Ovm0f)3CV#!D*
z2Teo*3^IX*!J`>OGU+EMSH4)n;IjZF6bNp?1#yTNfQi9jaYzD*L_%7kSsWw@O=Ka_
zmMkoeq=e`E46#DQ1y#w2`Z+2k6dQ!HBr?bt3=WTEVE{{{C7Vq|GFcb~63rqKv1|ev
z&A~AzqG7WrP682+0hW`?V}t`JAwPVgLrFNr-pkb*fwe&YXz_|-NI0N_HDWoJA1(XQ
z;m73xz7mF#O$-rB#Gr{-5(bYaVafO(Mgf3G3@TBHi9uW7zThg$LIINjiDf8t3Ia@c
zfVohpB7h+gi2MYCC~Jf=B)HOZLf&wzFGX?Tib0E5rQ*M8-WOQ=<?hQP5XGJ7g2N|d
zOJT6S3?gR40PKl@K))|ttThaNH~>E1Uj_B8o%>&kg(Ctu4xY_GvRFhsSS&mNWSC)z
zBx2EI0L+`kz+-<!7YjHNDMJL<hl81d*?<C?$OdjUL8bYx*3vbAk|#9Q5{V`wu{b{r
zk%GohaO5RuECr25p#J?7nH+$K!4d%^fs6x1ge4J>3>Kb<<Pfl+&@nhP+44)Jz7zO=
zO_8Kb5sUdXMJtqY;`&xpE7bog`xk?sW;&3XFKyrq3QowV?=$jO&Xh{|7k^*n_AibA
zLjM`$xA^@-*B`omi-F%V{-drxbo~|szh(SKUH@-%Y5myK0eo<wCk1zG*J`eu26uZJ
zOm}D6#J&$QLaj;wuV6x#Kr#3o+C+IkW&3Soz{W`uR}aTY-7o`P=**&VmpZU%w#0Fz
zgeu@Ew+4`j6)V7oOSzIYaOH}Xo}^;{fv7~f((L_Y5Bdt5-RFAGNUd@8G><U$(|xlC
zaX;o3@9cPU)f?kI%j~^_x6P^WXuDi^(-pnr-ugmS9GYz;-Hgf?Ck%!(?Ss4|t#TrT
zSf$j&z$}hWih}yCepc3)nYdn#EQ$-0FA-`N`su8&N=#7g?0%~)6G2e}&~vKCJ`6(7
z4L~lIw-!IC-F+=$-ElWEeDbE<<^vb%9wKKa8PqP3TMuZig4Udl;$N?uyfs>!ifx;k
zDBe^N-0zcjRZ|A5R<$u7$Ua8s4av&|g55a*(2Mu(1YB+UeBD7Rz1v=OG(HS!?5%S*
z;=*pIZFI3G@}q3#2Je33inD?lk34DD;vAp&j1;KkzeM;H*;bb-c&GHvL*t_;2w=QS
zeXKIACAk0Oh$Mve@~}cf-s;Hy6qi4`kdSLvfIoFm?RIq;(sT`s?+n`)B3|YO??as^
zQM=uDa?I_fs#W<k7lnIo4xCSOt7#r|WJ3n5^vuTvS&I;+Exg;#`!@EboYtE?%PNHC
zc7J|0|L%gPjq3^meHRQ2pE{r^=W;x<YM-|jAd(yXlhx#FbKqE+m8lEF$Qd>*DV3hw
zAb*+zpD$Z!>Y{Dr3Bx8S=*il0Mh<+sEWxx%vvUPZJ5lPJ+#-LRTMLm9P0=cyp|Gt<
zM7j!9-j`E5OC~l&YlW7pX<XWAp5#41F_oRxzF5||$f%ayJFa`~jLyY^{;JyW)ytvc
zMJVZ5NSr2FC9F0-tadgj$2PQRom!YqGc+DzTsvt1JgulqYcp7r<rB0j*;b}*`$6l|
u<0ZpW^X2OCJC^mmn_O;fGdA7fc9(I%XZD2-T>B!Wnq3_|X~!H^C;tzI2_{Sc

literal 0
HcmV?d00001

diff --git a/panther_lights/animations/lynx/triangle01_cyan.png b/panther_lights/animations/lynx/triangle01_cyan.png
new file mode 100644
index 0000000000000000000000000000000000000000..192a8eefa684d832d514b274c298eea07d46fa68
GIT binary patch
literal 7481
zcmeHMc{r5)*PmpGvKReY$5=vUpRr6v_BBfkA={YEU=}ljQBjjemZZnNBqUo36-t&Y
z70DKbh-fh_h>B>Tcc|xi>Uw|wJlFd^|2=bE_dWNV?>V1y&gY!_dwtI}u?}_?k`h}a
z004lbm8Gep;2R>iJl2T{?y^qNO8|g`!y%k2#}OY2WU?4!Y9I;731gCgBrcT<0C0!P
zG6F)LW2Aoj#@<vZeA5UL4b4XG$(ZO7v$EUudamI{@|8VlTJ0+P)i=gXcOP9Yd+S~A
z`0B|7sO+OVwqk^RIx?9*GgxmB*)#ZZKJMiWMFX!;5>^M4F4x96I{ofOL``=X_vQj6
zY4*@E`u?QKhVFY6TM7DuFWV+R{0DZeXDxQO<<p#@nU`Wch8hOD?=39MUn=>iw$u2r
zy`MhU+rM#{mE>w8ne?f%_vFLwu#*Psx%7i2<&PNgbi*-7{msT5r!HyaY?Ol)r{p-R
zed(Ywr;fCyY?D6mu-t}c-whqdULcBJs@f|r`Aw96I4}Y~7d;r-@_t{wdGuFE0He*m
zp1K_Hr2Z@x#qDsv*mG(QUUu{5_Qv`h_X6?;Pt9rDf4!VPul*r<a@Uu4J0uniMoa5n
zZ$j;l_h`;HFlsQq(aoiAi5slYWW4Kd9&U|o*fZ0rzuu14SW?v<CY?AEa2Fpfi^`s?
zY4MHK%TeD(@PHIe`YKyt%TEdqJuy1plqiYzbytjL#W4cJ2dkE{=u!OZANI`pZP=i&
z*#7Nn$XK{TzrTj?7(IJ9b<D57UGoTVDP$aukegdm?Ez?flIxu|Xe!zv!d7m2T3i>M
zq52hcpjPfI_cF8oqQTt=Q2?k2qNS#S7!R;i#kr`!t=9pDEbsfg#u2iKz!W*1lks|m
z)~pgKK3_+!+=39hK*FI(>DHxIcO{+oU1dRLEm)Bz=L?c-G^#DBh3Wg9s_$KNE^Ytt
zyNX*JPL^s{8=vM*FYH$<tA1jB8mlx`=v3S=|D?^Qula7IxAcz9gw{-Q4rxH6vg<A1
z^*&|7*~Rnf@SA|Cng>Nqp`%wHzdfAI%<F3}&OY>_>Oqe~Ns(2hLYb!BnUL)axApcz
z5fR@aFGcMNcy})atIlwHUV1km#)svoMImbO4O^lvCBkE9dLO_qwt?Ac$j`FQ`#~qQ
z(z3T}5s0u{nUg23k6<&|d5cHe&m@OTfR|D1ZpVeL&ZV5|6<4p>&J3hpsoNll%Plc@
zo2z||V(ra-1Mdv5igeciCM}ed-AJnl3-3{VaoqTh_96xQ>97U(>P1MpOtEneE1*`R
ztikP#dS5Jlt^KH`T-tS+6H=Nf88#c4m+H1X46Z0dWVcq6_(HoK?5;R0PFAC^L4G;v
z<Pk!FJX#@Vy0f3+PPx7%<E-g|<LL*rk2R^+Hg%6&$U%j+n>g)6`hj_KX4|6ftZ8S}
z0AUrg?_L>r1AUH)dX}_Wc^Dsbc5x!e&@w0Fs>|j(b@`5Co9o?Xpjk@FRrqD?fbq9^
z$}e|FEqU3WejM47Cn40}kGqoL#8a_;C?_xJH}@P6*b#eb$K_$Q7?02X$0Ft)O|e=}
zH0{0<rui(~AN}yPjgtLh>01A&C+f0&J0}rIlKx5wQ4v?IfgX~)`H8sCGYM6J;P6{b
zQlB!bbv!jH8jwR})ig6X*JoZBOSi!R`QT^hd<Y_A(r>f-NYXP!fK~g}ffnh)qV7x~
zH-7oKc*y2ra6rqh`K-7C<&iVa%2hVgTz|0BSP<R*f~uOo@&d=UU$zvpHR{ok^2=l^
zfDY``9;au?h{x+mMg}Kl60)8s5MN(fwpR&jle}R^+Lh$76(nzgRaLVfQDUZLwTkws
z)Sq>~>JuFkxv}KBrjBvf@cS(D=M6UUshH~n^3M&r{HIYM%Q%^j_j(50&lzgk&1WaP
zt?10s2<#JzE6$IR0h8g|ga&qf4)K_3*tgmFvxll3?@+8d(hOBIZls)=tSH@-dHKBf
zx{T^0o4PJ5*@}%EXn_sfuzLzWUNwV3egII~@)ah1NkIn=1yy#PkP1?cKXoV{q?MG9
zS5h$NP2ldu-8fxER2cP|JYM!6&sUy&SSp|>%3$E^cA@q2v+lQBfa%Y090im6p=aWX
zCXLZm$HPWo5x!jt`qjJ^Wla9tjN1jR^)Nl3j-Z4*BhX;H^XyRTLk~5#*EaWqGp@(4
zGkLYs{?WM`_NnYmop*M3*+|*Gl}`nB8{cOpv?#y8ooc7ngh@n9Y*Y)l)@j?aKT~90
z#xrmt^Hf(J<l(gfllON^fNR;G`vQ@$dv?^Pk0f>lC><E}E<J_wcgr&OT$h@5VUAe4
z)3rRR!Qa8xZZU>4T|(KE?H_tL={K!g{&lvRbLIIWP5j7y98LKuk>gV=+axvH=+wHz
zLpCOC`d0Vga&fvw$MFG!D@wXk7Y<?MYx}UHudfXicwg)n_vEGG&-y*nND(nUE2EmG
zN4<7gY5g!ac2`Qg?f&kFZjFaq)<$yxjvV^Eq_k(!X`IJ_g+iLZ{sf1f-eRkGskxnc
z=ijJ0QRak2g-VaeWjp_VOMTjS%j@RnaT#vIW2xQ2oSm~kt$2HeWS2SRi?atePqqhR
z@%g?sdB<T3%`F5#rt9>klF6DlugN0wN3jly^V<p@|8_i(B4RXMdH9*Ms1{;t;-*qt
z{%C6NLGE)qSgx^F%(bZqb(z|x?p;oL?a+PgSC*zb6ZM-uUQ8?uxXJ74sM+(G#T>}!
z{gx1vr98sCKfGM-Xs|Qc<W^GjC&~OBm6WMU;{I#fCz`}?qeUGzv+jyKNb0>}$e+L{
zC=6-?EjnHIri4_UHz~E#Uf6uvqeM}ApxAdC@KKHvh3BQ8Fm$Uke{s{kt-)rq!L?@S
z=8*)_^zx+`XzSzOy;{YNx^zQdwbT{6KNyP7cAz1R+tLmR-%#bIEAoFYeM?;?_;|G?
zz2tEA?v`_sfL3O4`b7E)XEuv+g8jqAHwfY2gX9!3I!D5P^OE*xh}Rulk<!T3eGJM&
zXG<UP*ituCth0H3ziDOORCD@6-j_0Q;QsajmEz%T>j1w&WX1sgDQEIV;?GTOjMrIw
zepmD9oEM+a<kq#;ia<z|dhV?%bh5YcBTdmpnYe5H*1mF2Z|s(~Rz%REmHg{z^!*r!
z(;4Q6^a~}yVJPzGmbD@Pz!fO&f$q3vbWC7)_62l2pvbId0MqtVOk5{PM*P^BAe}c)
z-S4}(UW1m^eCbdZ%}IMxbmmIkm2bH|XXpEQ2JYJFjg!-_B2kUMS=`6sE3QhvJCY=>
z938;_*jU4Rd@fO?7oK>W^v#)yEgf#KmU!J*3tQaUqCWh}w6QjR=)>6dXFHmgj>LQ^
z{qjj>!Fl0>(p%Y)p6D72=H8txngz7ATMwj7X+-3G`0XI-5z)fDwbbjZu}hUpaA4(a
zZPLNh+cV8o9^Mv5oaK!4*AF;qPhUJ+c5lGIL?5N&X>IzvGnYK~gr7Z)&Lfy^k+!oZ
z<sU5h(6Y9ZTbnm_$*SVpNcV=^amZMLw(y$|#BTEL$y=_IKvC{Hp4Ut=-XOx??oi`b
zB|kN$cXxDL<Id^ruqlFHZ-DOu=5s}r(A#gtfRw7L2TM@q=qbqVu$}J@yo$K}QO9qT
z66khEes}nz{3VIU&||?zW2fYI!Q2b@a|~i*#=UU)11E+?mlQt>M|8M6WHkT)Yfey2
zOdPCCO#V1q3Qm;S;fYwwTZY^2csmtprb;P%ZBjA`z6#U?*||r7aO%~@7p2tFBi7k&
zN!qAr(Of+~-saoie8=s8*a0J%uYjNsrXwd&?pyjs_uE|N$m!699icOTb1uH6L)sE0
zS)_tzaCow(?AnS~rcmPy^Tylz!nHH^HL;J3-fA1HQ}20iu<-Jg3rlV75zxC04<t`=
z&vIE0`t)Yxquus9#`MHmY^VjkQywIxkLfCIQ{a*xSr+qSZr%1h7d4?kHZxJp0Vb%I
zntw8GB}IKupj6}Vrw+eY6lD*;mm8!?ff7WtK;p6p%}0jf3i(z31sRsT5uLa8jxQ#^
zNNTTMW1bL}a?Y6bb^PdT-9hAD1HPIF!SKsT+R+Vbc%FG))zdSC$K|<zWy-j9z~>IM
z&zs~qf2Wh%)|{h_KdZgaez<8u>i$B3&50M55n09t()GE$IwW4s!MuR}>9Q-QABX{M
zJyq$)B29d142J@C%)IrRo*qM<T8;qR&|%%0l?_S|OaV(&!D-*sb{~eoph56NhA#=i
zr7;E5005xBm&?Qx0!SR7FNs2>V?i@@jUXVEhy@)$+rn&_CL}+q<slZy>5v_ca43MF
zM+EIPkkIF11OPM=2M^@Z0_kiF7Yka&#R#5P#841$RfQ9P1-aTf08JPy5)cJJL117r
zE;R%JGLQi3vxsDjqpA512*EEb$dAKeVxZ8_&`?My62f3npm04sJtzzTMIgWe4KO>5
z&cSoRbhh#e#CHr+5}UxHGC5QR9k_yt_hkfgupp3N9r#ClG^VZXpYU|{4;BP`K)HA(
z6b^wwX*B517Hp1LhydhAK>yW(jS~z_P)8D*5zHcx%tA<Xj`GhCM8co;%wSgFYC1#$
zloUv!2~^nvukc@7T3FdS{Asa50fkCqu38Dm{za2RCI3a%FR`txtfupGAOiD0aevYN
zBllHhftIZ;#*{$_UJ1|26boAMk0CM$R3c{ekxYi;$po@4SXUQL2BQ$#c(5)BNdyzn
zFcKbtfa#F+kUv3L(b*h4oj_WF5`aUf0vv=M5`{t&5nzNi90f)ZNkp)&FH#%qi%09~
z!nDaq7!m&ygguKY=t_Lx&rz*F5d~0iG@J<6L!iKD0$vx4g6Zmj@j7rk7=}c`br5(m
z+80S&g(4C#<_s1MFDNIKhNqCAOgd$CVTEvvk%JW$gn+>Qk~jq7Ib?wW7POB_59a>0
zfTPk#P8|FSn*t9yaG2nxtBpqMBEF}+iswRNu?3yDf(eH~kl%4v%7PIjBOn&P(x(D|
zRaikT7!wu=&tb4|3`QUpv;qNKv0QC$p#JxwSW?*njj)xD|IzbKq@eG=zP|#2)YT;*
zaJ6kQc*1ulY<vjmI|l;0?@I(fJe@)kyx%_>>JK^fzZA=tOhyp!zB*unHVg(v5zr*C
z9*PVD6G><Ul8hvi32435H2y?qGsv7!Jd0#R5o9XJM$ka3*#Om7snqyYJJgS~!V^qT
zLzo^IfyBXeFfarLjoK}^!(brjKc6C+sE5?`CE^7Q=t}^jkh%yk9!183kt7&V7f#Sc
zp|sJz2K|3cQ5OS)W8k{Krl=2H*|`2Fsy_68mHoTIPdlA}n(s2f4l3A?p?~hkKR8?I
zlz;H^quu^N4+7|ao%}6+|E23+y8adee@pq_?)sOmzs11cQvSEQ{@>`5_-m#k(FF&+
zP{G8O5W4=TVA>P)wXrZ=o%;ZvOzKu7;!I07w%`vU<&|p<w{UNWKq$tsvNaQXC9Wv5
zMh=iUiWi9f!!dK^m@sH7qXA&`#7ZIpL#Z4;;L3^h08dI001zs*GBv_+ho`e`?cW$~
zHn)Fmvq61}ttImu{2k%xKw0BR;a<~FwJl$pq$T`bdv5T6dR^X#LC&bq`W*6TQpv^k
zJt~I6MB$o;>p0?@>-#0-Jp8Vo>piBzyMFzc`PaqGnC*{-lnQzkA&_ytD4o&hE$Q<j
zs>9~d6YZY9jppZp<57`o7Z;Ct_2^59-k%l)Jb7)H*8FnUk?`<}dr+5i{m0c2vsC|M
zU&7Z`=pu|8qz)du+$eoZNgvzXaWb~+OPxDcDWPkCuU7S#$z3NcE$R~VBuf3byQH?Z
zPpG1uz2vMP(WNj<k-arb-KLTXN-i2}TDs|$YG%GnH<OLYxZBxLcQqncq@q91E4S;*
z5>|ijne}L^jR}j5vk&TXvom@l!qMY#nvTnZ<yKF&N-BF>37tOuM$?gh6Vr9L^1?Nc
zVr1KuH*qOZGy9$@Iv2dG@2HsJI~TOozl`j;XeknwK7vir<Y`r>8>{|axPU7XzSFcx
zgST<`V4|GmQ{@zO-X%_okgDtUkxIF{_bm7tyotD!>V%K3iIb6sOD)~LY{=1cwAMQr
z9hcJA{h|icArk7E=;%MYtvjn=u4A<KQoB@5q?^;H@dUDMNYnPttSR0>7ojiXX(>&R
z8Z*_)(sV2Fkz)rPgszXPYV!8XCZ#rkxv|#cV)eqROlhwqxy3b5Bk`}iiVMQa%?EHo
zjpG68yrZ+5QVqoxC9Lz-*B_I6wFX)uT-&r>oo6?znqs(fG08elUf*8)gvp)p#tF}f
zJDKSV=apa0r$m06T4=!P?upb@19h}42DNeZ+42%%M<n)s0^oU(pWd_5MAjM5Y=k^D
zXC>-|Dw=lBUT%(pF|yY^56T;lG|C$f4dos8_7stb2;Z*4z5OXnfgclE$4lVO+pnXi
Z?_HQ`HVWS|p1krhSee<GmK*yV{~wKsEy@4@

literal 0
HcmV?d00001

diff --git a/panther_lights/animations/lynx/triangle01_green.png b/panther_lights/animations/lynx/triangle01_green.png
new file mode 100644
index 0000000000000000000000000000000000000000..bad06bf75520795a5a33f4d87238b7177adccfab
GIT binary patch
literal 7309
zcmeHMdpuO@*B^uk87Y^<7^ZR=W{km11~KkNZn+dPR|Yd~gF%FHNhQKj%C#hv$}MuM
zbkR|Wa*d`E%3TU2y?fL-r}Mt=Kj-tlzyHqueD>^TKhIj<wbpm7{e0H*nd5f0=Ay!~
z!XOYx)Y1a)06aB-+kK+|@C$ZK`UV0CO-4GovK)xvkig&o8r`1?VMPQ|AyhV<1_H4M
zubuU)RZ<Z<_Pr{_jW^(aFz7C(>8RV<VXG34CHu|cOZ}m_@3k$36QGirL*Ktm)_tF|
zQgq6;&KUIj)EL}}6SKW<==t%Bh{2)!KDC*zx8_!Et4*#c2CD?woW715>C^4QY+hQ5
zF;v!nWVkCb@7P_pmHAfbrB5p>UmACvTnH`N6E7*h?{nm<z<rr-cG$-pJ`|3xvHn`E
z-IV?C&Lh~$e756>n&(|w`Is-gzT@siou>|$@kcsOotDE%e7QZhx+W)kTxCX5f^Mra
zY`^nsJ3Vmf@WV_wK9e((4jj~_NZ1G;Oi`TPqM*I1c1|JWqmp?GyQxaO(<C*^WG!}Z
za=Dm!|9d0T(>q+I(JmwMdzwM(hsnw1p2<S}U6E^Po}I_@p$D$-I&ym?NkPH!wZXmU
z)Zgp3JykWU7%dt(5vlPGOMY3c?is#+<f)7K3*lX#XBrd6)qSnL#nUrB%0v6}(~c&B
zZTtIz5=@gZGyQrG>)dVnOnENJ_wC;>F!K7o*{R*d;GWyQyGxc+VD6`<``$W>U+?hq
z@)5DwR`*V!Ww?Q_6RDBFd6&Mce(uym+oCz1<>@;JaiMWr-+GDF!$+cJDugz)XVHjp
zt^1bFl75@JSl7+ZB6+=D_KNn2+-H=+W+|RcGrH54W_1PsoaC>VX2Daicj5-pR(xYI
z9@ci&u`HHv4_jxcI#Bc!%s}H}nM#9`13}c^3+eUF{Yw6C39_=VEO)EY;;kx8Cv(b>
z*L$4_dFIyeTiw2X1ougX#Tj+eGHvjCt;i{%6^$KvVegwx-!&0LZJv=AktRPpN6Vx3
z4qSdRGv=Dv`Fr`*+k<Cb^mSdaRIGDms1Als-B2mc4Ic=-1#g%Q+Fg@jYw+%Q&&%|l
zPIloo=3q?pchTqfP911WtBjjYkaam2Wec{kO(6=9kb<Cq0s)d_EpZGsDkq9*JsGn>
z%}TkA#PLDgH$u#cw+m@bF-PUxR$Dk@Iqr22j-0rFni#>)>|q+35D2rq3H?VCe{;(B
zQfLW~$%u$bA%?!HNH$AUxOTcD<l|KhXKT7l^SDHt;9|Y)Rcdp%ztWZqCgPin?pDN%
zS$KEg+O5j4AFN7hZVak3YU9No6|;F@F1?Ap^tYD^L&x&m5qJF`DfKq#3|!TFI-;uJ
z=w>qrd(iCM&|+2mwBn^1WFBSjxi_qIXvHy1YE1rst!-f(ZIB~UZ{5&mdCpX36(zOs
z@nTfoh4h+>+SFWZ>y?r!Vdb|LpS?fZUXisnd-k5!jdbB^imZB0<X(o{mzhU{lCOT-
zavAPgezKQA7_Z*n!-G;&elvLcp8RjsH&ZRzm6!=q<@L>$dvs6Mi_JalI+hydW7E)I
zeZE720u2tDx*V0QVRbEPVXNyV@8V>2^REz!!*kcRhFHgI7+0$f2NCd7@orxaR7Kfv
z$w1pj^#=RGtPf(j$SvYDg`}-Ldj!z=lg3ojlAxXFj&XgTS}zv?w$8jVrEBASy_MGP
zYv(9Y*4lQRWm)fyIkn@9=b!f$jCMTseLV#_R507`v}RfiiJ<ztudh0}X<)c#753#>
zQ&&rpp$nlO=c=&zVCj`KmuY>Oiq`R(hyCJPL<^UTtD8M?h4w$bIR@KWlbK<ewzwm-
z-74BTw`k+Q<wo=<acS1qUG-r2UYP?E_AhM?whe0SN>?1Y8JT*uGU2MpkiO(oQ(irB
z78;cFSq8i#>A5cJSsf>AN>~v8Vteg`Z`P}O%#vfujPbtBZK;!mkOM`0G?!Cj6E~~8
z`!k{}yLdVx7ZYOBb?)g#Q*E}go+d-u*DyB>&MupY$$vnWx))mX)}E2R4B4t|w?p;l
zy;tLkid%(_AD{~?`X77hqtcc<Zw~HO@w-)7AW?b$+S<J6Dbve-V9JY^QYlCqoqDk#
z2mESNoILo+_2T&^XtPLxbmgJsmw63UPkCQ;;HtV!@8#&LOFN*)514BXQR;LnGm=AD
zzaO=-DAsusK$i>iY>VAK=NIR7w(fDO1VKGcHn)IWS{*wVy6B|p5kks#Gs&s4m#y1b
zds5KK7%W}6*#>vjusi*Ot4(xrg#6^C4_iNvk-FTb?J9PAtW=~Od4iwnx_;p1>(84&
zl?p1oR?(<yQzf87&Vn)4rMX9yyFbSk*is47_3s_QKGq(Etol-ifh3rRvHcig$IEhP
zXamOYo%TS}EzC@fM3jKT1Ez05_c_{=k==yq#FHBtg}L>yg7Xg)CAKwvg)E4bxK^GV
zltd2Pm~jw$+4PJW_6=nyZ@v^~Ust%rNNcC;{ETPmU`_a$<1P~?V$%dXwuE>IlSE>C
zaP|3O8cRlFVyh!TACC3pJd?yRpe_D}U3=6K!@NOz^sa?mRlBZ7(3Q-RmsQOnJccdv
zcO|>>7kTZob(I~oe(nZ_@!sB$oj7*uQNFVN^ST55B4d}XOqCp-3-4-~TxIbR^CP0$
zTMT^R55b(l)JwN4n6!h{j#x_3dHUl!r~95*5X}yL?WU$A`F45M^R?WZ2&;ZuWt4K<
zD(R3QRGmVJ=mqaggxVLPeH+x3z9mfu-Mc^$-VRpZE=IZH(%LL-T4L(l6*SQ`F_f+f
zc^R)#iW?O>?#iovd3&hNck{XPTO2FjmFwxvA6by;RD$I;dWDq_c<aY@_w`E_9jVzS
zb)xje@zsINHyF=sv%58(XuiI<3zO+6AyGCwxMLu}MdZ^S5?;VL)I2G(w~T0cf+ngG
zfM+RU_<4#>RSs1{NZ36BjyEcU-LL~9X9ziJVv>w$=J}(#-Vx8Q8t|qp+HL%#v7s(5
z{aaO2U}wgQ-N*Nmjn9`)f6iL2G3qd_>d*F)rkHLL`+&y|FD!oNkD3xpD)kOYZ9doz
z?_L?d?tIg{wjc>w#ey9^H=vXFcFWD_6AMP(ns<|;9~b8LN$E5MC|oOyGIbs(q^L?q
z?hDUuSY<?g`%qauR@~M$kP`GrZuZ5^*vNKSjX1%Ms-gQhHFbjtHyT68{yx~DK}!)9
zlm99$J>cRy7st1eotHA8P;_22|3V5MG3unQ&}izRW6;wU_ih<^nhY-)HSha2WH(bc
z91yZF&YdxAZ(F)D+#-UH$(TA_s4TqPHQqkpU9@@m&C5WYh~SZ_ih4ee4u$Lr%ge?|
zH_lHRT(*CdW7#RKrn5(I(jWYLUSEG-WAbM|TKK+i7y8G(A=H8f8@2o2xT&2fKDJ#^
z#(FTfqM;M*i}|`)CRbMBL{;ah0@C}ZqVt@G4@*OH3T8~aqwXJHSrH%ITJ*JN@JRFG
ztm198M3+qI=GvGIbj3BO(ZWX+{E{v7)#Hm%yV7SzpEYH)g)dBuSd8AZeYYk!j6XD+
ztT(R)0`Vl$O-$@8O-%l1`+#QeLUcOLqSZ+5!6C<D)hw}Xt9KMlLMtJvP}>6uP^aBB
z#^qu=vtu?AWY0)SncuD9a2}Cf+<oBYBIvSLd=(TF7U;lAm-wD7dEh=f@a5_7CHe4K
zP@yxaY(Pi&N)GkX8zeHr1I%}Q0<UR&)~w~eO|;HAn>!(ghg%=@HSF$OFjyL&a1P$d
zcNjV^(j$6;oy!jH>Bh`%NpiDyNbWptE^-?(uhd7)ex)ZRw~bBfw<sA)ZoN+`OnA4A
zW@-Y<gQV^*Gg~o!NKN>-&AY~lcq(c^N+4u#L86Z?22JHxhYEqURQrvDwiQ+TUOH>>
zEapk8A!j*b^vvTL9<$Vl%tGVfRnC#QhJz?WgR!0bWTUSq7)L~S;yemGYo=$(Llya;
zHE)iU-<Ni@*N2QnU&j-2JcSI-o7<(2qwc&D>sq>Goj7U{lVfbKxiSBlE;TOiV1eI@
z>1#Jn_Xt7=9x!I?Q4_B^qX9qp*%_bd=~t*zYcZgD-Qd<aa8N4HpnszSjkqhp21^cL
zXb>p@B&r6R5ePKuAdtQxJCI2Bqp~0*syCg9gU&XzKp}Jr4(fs?AP9jbR3ExUWH8k+
z($<L_=|{#;poRv*`fMx!z@V~-5H`b~8G>cwpzF9;V9cG?ghJLuSbjLDE5Qz85)e#<
zXlrO|AmFBKdYBf}Kp3JQOrc>N@Mb?DfHxe}hs6rSYHEguhiinRGy;OXHIWz$MiZf>
zsig%6B;X+tOcs$1XND+oA%0-ssUhTGdLWA)z=UuyiKKu~77hvp)**lR#|R`4{)A_S
z{A2;(Lz7Jm)I@3^G#L!dUo}ElreOfcPlx_jjSwf`JfrDA4G9PhCR0trs7#j9uMiaS
zpZbBJ!T#&vP{^88e<}kI4FRko|7OzMl3@3z2A2YFIwNph3n2S%k}NvyFS7pT8+T<r
zoL?OQ)c?f&oAe*CuL}cG1OgTxKn~@)XNku_x%RP?05Y9|T^~|)^r+eh3<-{;pmgEd
zL|qgdgCvpRWHfM55n4zZiTDeYB{PIYWRj^|C;(i84&a~wbv?4S4je($)q!iHX*zHc
zNt*`OMd)gg5hR4R9#!WTh<(9ypel*}zk0=mq5x2OR2mvZp#a{XkQlhO9$FVpB<m32
zXp9y{mxLthp~;waC<+;C77)xJ0_mhPh~8AqK&JQl0+(>?UOP)1R7(T#*NmM%kwpU(
za8MgMGnD<;f)kxVbz~8_Y$A2FbdiA1x;jV<N>>;4myk0xI0UFfE+!J8f%<{VO$!zX
z1|XKmty2JCT?2>(YZ6Q)vI2sg0s{PTP_9c5uI74qL-c<n#eyCJNJMZe{%6fQQiFcH
z{rCj@>FY}n$a>jgiR2$fLWp5h%DN#y@5d6^hsgA%0^j$~g8E~g{=X!Pgw~^wi9mue
z2yVVHXd0ZTP1J&GQHew?v>pwC(fbkJpXeb0G*&n<n7Y>+h!lthD4_LdAiLM8RQbCv
z;XYI@PY5k-I06GlVw{j@td<TIt)qs}!Xgk*&HsFeKm<sN4iOF4)h46i+B7X1T#o|8
ziAIyj+EgM+O9w^$yVL)d5VcTFNIfh9iADam5cM^=8`mF6)z|zVY5yVc%T5PS^J5O!
zL4gff^UodmCuiJB`43({%k4jC0f7G3$UoxuU%LLK>mM=jkA(lNu7BzJM-2QU;eV^^
z|BWu;zs_`2CeY}G11Gj!#qFiQX-|M;ZH`|*_kmVS8jb)Xp+F0_5a7QXCGO3`E;bAU
zCIwlR1XIBYAt`Yl@tN#K0x%`RGIeE{1TeTq1JHWMN~J);=`0@zw_|mQ6O#ggc*&Od
zy-w`G=?m@FZ|x+LS|-5r=px7j32*0(92)32G}CQ}508HnQr7D-t*)O{E8U~K6eKok
z>~m&hd$Xr)%!)=eJXTR`NY3^=c@CY=xwq}chQ+wN5}8+mwQ-?y+F$P}a5m`e*faDI
zv?jG&XwJuZAlNv!g$<fn{3KMj2x`7|y1<3Q@JGc|3Huop&hhcbII&w{O6s8UJ7RMp
z?tHfn=q!c3PKX#j$Wt5Z!7(O^_BGf|96bW=2Z?Q6Hr2iONn^{y%BwJxG+)f~r>>9N
z#ytzo)nK2P>aTX)9gj^YjWiTwE}Bgryf7cMY~Q3X;s4z+&;+bfoY;J6;e}wul<B3z
z5n)cV?O{rla)(F4@K*2lC@oheKXIR3m~Pk)K6i7Z{BBE%1>Yx)EZj-exF^rs3nbc}
z;7+OR#RZyJL24qmJMfm`?j=iDT(lR+EWog7u(XFt<(ZIRyjA%#8Kp`b`*Lz-uS=U`
zy2Ple4AddsLnR2G$iGiIU7~tUq9R9mc{<PMg`}YV-7*z@cF}%Exw#GHm)V^3EQkSH
z8J4_B-d^a<QFDiQZ4NjS0%se+ECl6Ug}UPfo#OR5Jed$28v<+NlQ$Ath`Z$!zseEK
zf@rcg!P+*D*a`W^>e}%h<e)R}h_MN<8~mOoLjLic1sgu!@pj@p&)IJ^=P$F`5IGwG
z%CA_$aXPTetmXjT7K7AHiMBB>=M7fG7x)a~`!;hn$r&^^>Mw3I$og#uTW};UoC7@a
zs*iV$@7y@+c{jf?K0kura6vd`YD**MR;N2yqp5?vv~MGQ=*ZGhN>hfQlq9!`Elq9l
J6~<oi{{_Tq(~<xH

literal 0
HcmV?d00001

diff --git a/panther_lights/animations/lynx/triangle01_orange.png b/panther_lights/animations/lynx/triangle01_orange.png
new file mode 100644
index 0000000000000000000000000000000000000000..89a506af26a94ef4d23fb3a51bdd67ab5b3d8e6b
GIT binary patch
literal 7384
zcmeHMc{r5q+aCK8$`Tc2j3r{sj2VWpWj8_-Lw3!|*qa#)N%kZWB}>T?Etb5Bw?vi{
z(n3-wBH2oISwg;t>V50@e*e73_x=8R9mm}B%za<id0*FgozHz-_u&-I+Dwpt7e5FD
z62zJt+W_CHz{iD`8yJOcPp*MLd;_6)2c``n7#u+Nr_y{VU}i`F1x#Vls2~vQO=$)#
zXh>5iW_?bK!5O8U%;7inOz`W{oWTuC7^UvdV;DPYB_#{aZSf!nzx6NuAHFs2{VL_s
zTAAjSFnTWZlR=f|x0#(?cb1sP-E;n!IyQT>cQ~Y<SHR@?%*^eR$HI`$D|_o!?#Jtl
z>buX^Y#ZKJ*N3~`+}u2Kv<_a;2D=B1pLOUdTZvpTLG+3jHcy2(<3o$3U7YWDTZRc#
z-l-GwR;)6&3cEJo4b2`G4=Z}e$jtMI`xgH~Hn%KtbZWFS!ir#aDn_e`<-BtOW-{En
zEQG))Son0EorZR~xUgAR)^~)%3chWBx>z1^`UN^C=kBoMkOQV5`1<|lIkQ)n%jQM`
z1Ftn~skEPzoRKZ5t$FUAbRfKTEJU{UHJ`LfR#}>UqQ{r7dSa8hy`v>Ts@G-=ZDquV
zgFbz14SZBXI8<|apVTV}v)3A`pU0%m1S)7%uEx-!$5q)rC(AkJ)5-3mN}{5+-Fb8&
z!o6)}4cz9sPnG#nche2!&BSjA57y8XA5Gsfxk*{6>^(n{K6fBvS4=uz^6H1iHRhmJ
zle}>Mo}*q8wGG{;w!202TU&AnYX#%xwhxTXER*h<ZhLw-LsF<ndU7TqWf)pD$c4Jq
zoU$7I#inbG-SA<@PLf<y1We?%m`afhUYNZHWRBRa5y!@!%5d1R(<YHC;0TZRoMoD~
zSIc`uVo}o&F-hM3kH|+6X*LQcyu60aj%3(AuNum7o^(8*(o|#Pg$Q}Q%e$!Rf|EC+
z^004t;RR<}W##LO3+=MA+1ff2rHeOP^II>bT}$IDl1<d?Xt|Pzc{AoWCU=~g6>`DR
z<Kpp8{K~4lwXH?PrM1i_3TZ)aLegD>-yR>Ad1bBhamdy>@LG2Gd~8;4#ah@ZyL#23
zEfO{w<+V6c#p_Y!heL9bRIqp|h{dJ8O}1PR*)Lakq-7_yQ`RAc!<5S5aY}|eiguvI
z%&ohxMJs=)Y+%HzI$+B>jQ%`#MTUN2aACXGlX*Y4cwJ9VgD=7hM^B)4>yGF}J)In<
zq-b2f#s4mKuvlwCsm)0qHJp=g*A@F}C@1@%SMh$~ey{~7gBAbwMqt{D+4hu!86KsM
zl(L#&`}}I{BTpUX!C~>EdYYlkv`JSPe<zIdS&<8wX0P{DN1=n7Fs6!u1MBD2>0a76
zp(A?6*&&5EB9F|8Te1#zX%Q)g-oqhbSPc4J_OgT({B!GiU(aJ~{astWD{XdOPJ2@+
zqF6bjgJ*=dypmEr)B3LPV`g<lnli7*nb=Wsj56lvwGw)VieTc)k9>NQj)OGc_ih>T
zTd*?D<1Ka)H#Ej37NUfai-Jkb@4GXOkt<3xyn6EFCUkh0FV8#pg8kisv!mx1MZ-Hq
zxRyR{aWZ!~SLyu7m%i?C2+T`bjyb)nDKb7ry}~k*pAj!9zwAn}!j%kQ!6mOUB;%&o
zK9UqpjgAprIP%Y4NK7S=da~?G-)_ae4UA?IN7;L8xk5u(HX}(;F}oc-fpZc>^nHj3
z_^d}8l0uH!<1=!P_fHW25rJ6q>qf1o_wJb-GO1ikX-<`TxwTiXGg->W!rP1eBJX|v
zsvY?^tzePL^Xg_KeNx<(%g+-MGG0g~YPdd=xe1R4@-Y9X$M<B8)=qFFBmKd5+1<s;
z*iSV?<0{S_KU!a|lJ>lA0ghKY__6gvqDw@b&n-85uYz$oo!55-hzAh@RZXt!ySnGR
zwtngetM<p1hPbDDq?SLqGkV5YJS#UPMCr@@&Y}p*F31?W4>uOqC5OQ|?x>pXKTm(C
zDI+Va8@p<nkn%^|?B^|7y<>v9UfwDnbTyNy5GM}9hNwVkZ^u!kt1(Rv1V>NWeHNWa
z>WkPeoABseUhCfc)_L~(QG(naEKYjhnGcl{<@RqR!%M?fPR@@74(E)T8@?X@nmL!_
zaW*a{)JctqbiYV$e|37VD3~+UL2O~)O_}goq^8%UYA>GaeI8FWufPjPHW5{wCD@NC
z2Di;shR!~?mf5oZvXc~{)am-(A@O+2H+wLx_cQD5lg@fy8j^o)d0s?M)nuE}ZA@Zr
zc8Afid->z3Dj#t;3%P_pg4U9_rm)kQstu=@^DyS8vJ^?!mizc~!;eFXIT`ld+pP1x
zHJVE8S|jUPUQq{YOML2U`E1SA7;d4!)*#)HkU~UcSC(mb2$+7Vok@0!uGv~X!J_6Z
z_h<782opY_&E`n0k7r5*p9R?{BFX-?Sgt_B-2qA&&+rTHMmT6gGqH7VPeiZ3tupW)
zl6ERHwlm&W?X&_~_cV~=jp^XYHl(_jJ4;RA7PlRUxjSa~CH=8pVQTMGkJqv0m&d!c
z@$nv9>MCCPG@T}*KD}Gv(wno|25XN!AqO)h>AP}$&qs`05X&=e=lesr7jI_#!0Sot
z>t%gZ{%v(DdzqD6K?Cx_6&(Hh8me!vz;UJ#%rU9FgS_C2Ir}4{z6opCzQ7;wB3lq8
zgNsdLl0@P-mh?(BzSLq*+3_Ayuxo*3j@9JOdAphHG3l$&OkFH#((*CU&zh)9%0G(|
zJ8bRfZu;U59FGCnrrM!Z^W}#0m&{V*o%TQ+u{ql_l5qU7a!|8`tD>t1A50JLTrla!
zye%O<gM(nL5+Z3w1gZ|KKfZ~YZr1E;H*&vX7o@z<C!xu?z-<hT&lHU^b+K}R2>Rsf
z=IR$d%3U`czHWq%M9S)#U28$-ISuC0<ee%wC^M-pJnbl3aV)v`qJ_`H&SLA(=X?#_
zH^6nBRcmCB?#Hi<U;K;B>t7=39SL5B9fOAyi=r>A%8YToS^d;kQGNI2Om%34UsHIA
zdrae%oTZpBlrgxx|Bsa_7ttoJ!Y_`f$@`M2Z?d!MbuzRR22*Q2ChqtsfZpvMxSb7Q
zliu@W%I_3sU6%Pua-no~-%6xS=>vn1%DDpfKj*qjFPko?Qp%3V3ZF5BoaaVR6(zqa
z2{oO&6TjmsZZIRL?S%eC{QBe<b7|*9bE+->N9#Q!I#m6UaE+l4r(w#g+LA8%?iW|p
z20wV`aECecKX@N5nhX;N;c8sd+vU#lb5z+SIoNTjyY@j>v0#46bFuYE;>=Zp(Yp3U
zozh^;#lg#5RvvqKCAt~eQlA5_m4C9XF!(U>VjO#WFt^||^r5Vf&DpNv;gSm36vJmp
zd_}uIv3l+GJ<OyoxAl+~wiTDX9N+1e^Q0F`GgS5?jp1?@`ZNwby}@=br_J}+r*6`n
zHiwue>5ba2(AsWSeeXR%@>&*-KOcW<UKRW;e5aBI84K(ECLpqNp8wpj(W$cN`S=N0
z%gN}vI>DE_UALTDY5X7^(`s>gMcQypGbLi;jW!6x7DqEO!eNb!e)f7mhj;0CimrL%
zeyK<9wuSrBgv3|tWsCxEf%ieIonj$)#Y)3sA%*mCUaMWHViINzl>-CK#8(ZE91rsx
zHrTcbIvNyU!%PubPZx7~$O`C94qn_7JO#?LCl>W+@L$iO6bvE|=Ujz3%EpXgh8d=H
z4=s;tWLnlU!ulGUyQ&r2X7v_F$L#3}9AS_dftP}(S=lW5%MPt6(UXpcY~tHanF-tj
z&&YOB()%?fq{La&Zu4s+@r@6Od9lOdR1+il%itu%BGYBVCQ9s_xJM<PaOT*o1UKW&
ztVkD42$IC50^t)@+t<CHPdvZEt02StMflT3{eh)(L#dA|*-Vo{&gU7@R|if^S34v1
z^+pu9Nc&e#`<@VBi*(I(tDKl3y}g+O`sOiU;r;cA`jJoP=DloBOR?qo4h-JA@%UK%
zuu%JA!GX9T^YAP~y{$DlFVK|8%g(vpuO>>%lV9?Htz6~(BBG6s+}+>fy=Q9Db7G<&
zdFER<s0vMQoEAQs1a#$VG@$!-u(HIE{C!mkWPc(>mE{`%bmt(Djy@}ZK=P(A!9<D&
z%}*CHRb2-G)5yAz!|GOWs{kX4C(S&RPO%NO#*;$5Nm^uxz8=303j+Z7QkVoV%h$(`
zfnn)FHgPe)^Tsp`0^T%Xdh0?QtZ-l>e>w$>Qbno4p(ZR^kQzjfAFM+sQ!zHiravHn
zU%C)aCNls7g9Qf%s|F)g{plVsgqD^T46X)KQ-cB)P)3L!lfZ)dF=RI&zGE0u7$iC^
zfJyWB18-mwi2i|0T?hnN2mg%EH^9p3FL*!34;BDEU@Sra4512#`TD|s^<Xeff&h>o
z0sTi01|GP?z-%ZC|3Es4ViH90W6J&tK_>m>9}r0Q*-VE_f>C@ZzJMtM*cI{HmS$Kh
z++Q9W6nM~l12(+?vVXH=(x`uv^;>KkE1T*38VKP27w&J?KXczS2CS^CFvk9*z>V;*
z#=4M={V`;J5{-=6d{jeFQ6vHi1=T_l2vC#;LLI89sRj({1T<9xP9&32<X@n$ehenT
zk3`vk0>D*i01k?xiPnHq)SzfBGytJa0ld)^B$Pr@S0fYE5SmD|<}VNj=`=u<1fO4{
z+JGVhP+CMy0-U5pgrZ4E3KWGxQ=vp890f%ph+1e(B#BC*5;md8B#fy)-IoBAljckC
zpuhtBJT@0L2*()Uu(}X6RrudCI3EI&3OMLOENOm$tiKoVG+&A>ld!=i0<DHdz<~j&
zsfI#psQ+zbPoXmaC2n9M;Ht>)xEp1`0LcKv5;k-S0Bm{yxnPXw6av$qj`#QX(S>Y;
z1m5u6ls8!Cdr{1348S5}L-9W}Z%aA){nz(bz=yWE1O{))7DFI?--JO3qI|Ci;P-ur
z<Vo=JpaAdp4?+E$r~QXwsR30b5s?~DDpH*YMIiv1i6}G)idNUqKq4q)vKp1NnZ{q}
z41X#!m_Vl(cmSCK*#H9C%m%EuNu|>7*1?{X4W8g?C@5SDs)ocP&=|NH1`b!=93im(
zeu^XwxF#SXH7Jp)Nrs}d07(-OKs-QA5$bT1CW?wA{vP!IHAPJf9DzX~e@{^dwsCO%
zEUFIdf0g~a!7nErK+X3#-~<H@WY}LP@(<27l=5Hv`ysdg;tl}xznlCce*dNGU%LJg
z1OLeQUv>RU*FR$59~u9vuKzc>`2W7sQT%{LFBrJ7L3I53fZHB7@qn4}=DiQJY*c*$
zc;X8%cVqznu#w&Pu(1mDgMdjMCf3S?XN*r`8=K&nl=kz$)J~>}1JlUgcjIaR+U!^<
zWN<Kz=?UKGSPw@ENq|7?Raj#KJnPNGr33yi_ludfJwyv)&!wh{Z@1Ht<KOb9&+@R!
zvZOilBxe-pS!0M66;YmGn3dlAK|2-dXtCPyVr*^{nJUUrz<IZmm&qqlBO1%6nx34i
zby73ZK%_msZnD8na!;2`!He|8uIa+9ca9%9f{ugF&c2twr2aWwF1E*Y$MkH?mMdDT
znf0FB=eSmP@PNW!#i3$D*iR3y6|GUztT__PliSFAGwe+a2f2D<SWTy_XHNZXAxnw&
z<oCfoB;=*>XVu2s@dNXmst4bjNYrj6z+YttBbxLUC511c-&?h@w~17HJ*x(FCRSds
z?=w}v`ZnFJN%r^(k?r9j?YB?qIUnr17QVH1*Ya1VcXfjA4+}Kzd?*_$`wbR$Ve#VS
z-4oR}xVrEuvpIpKv+qirmlI$EqYmyStyj-9MRevC-i4r}IZ|G@rp&erk-%?kQYfRY
ze@^t;rBFtWYcFwZzacYqYdqphLT74l&nWne1$br8bt}FFp2Jr;=`9Ue=l2Wg8k`ns
zLF}dBqCV7bSB#W8HmEAEcn>1q_LyQ%)|(blwwZOyElCiQkWjh;dGsna>~Mj0K-+dD
zRmLdewcO3AAotq}ivyRbR?i!bKg;50TSR#86R7c&R}`<=FRv(8V=k{KRih|B&F&T>
zvcwTw!uhV=RXOr-K)S_%bZ?3Xizm2{voN9>7u8Wex6hzzK<j)iep<%@hgmAO$Yo#M
zA>Ymso-1;lO&`yPkCe{m{93<=XAc}$*%xUs?P>vcRpjo!u-JgtUW%)!!`vx*S;Xx=
zwYV3`Iz~{o)m;j$bI<9yQx*+s^RvIv(}I!%PYb*QRVu=#RWypP3NUWeD^K&+u$R?0
za;Ky4)0EsHwM-uVZ-LtsmzvumqfXot6z#1Uk6W$|o12%z1r(hyoUUHwlG=CUvQI%m
R;KsXwHL*6nX?P^+e*kU@@UZ{@

literal 0
HcmV?d00001

diff --git a/panther_lights/animations/lynx/triangle01_purple.png b/panther_lights/animations/lynx/triangle01_purple.png
new file mode 100644
index 0000000000000000000000000000000000000000..b70f26a3db70382a7386b0becb9a73dd6d4cf3e2
GIT binary patch
literal 7063
zcmeHMc{r5q_a7v?B7`i{SVE0i%!tXplPycCiOllM7|hVjU?@uVEG>vENuh*MN@XcR
zB1%Gu%2cA&T2U##XVm+?b^X5oyw~sd{ja&M=XvhuzR&rbb3W%h_jT@TQrunbrNouR
zArOd^ql1kH_|ylloubRZyNqYjHwZ*DJIZSt+k+4R4P}N<Xu$xK9T^Hh0S=7<fpCVZ
za)XX(te1{oym)ZCAaohg*reyUYWK{zMq#)qBP{IWhe2QMRhgo?C8BjtzVtL*`MSD#
zn`T4FP3E0PvrgE7!K=tc&d0X~^XzE9>Z=ozEw>`iy!Z}7Yo*Oxt(rcutDB`d@il6V
zSkK3v^b@O#cbA6jx)k0scWCz8Q{>Qi`ot;+&bCjpF}$Y^&!bO$xT-#v!mORSQ5)IN
z-f}DJ+BYp-RFJgR^zl;&EgvrYi&yfviO3U;2li=d0?{relSP{~s0TM6SV@tobJHD1
zIFc&&+R5B%frh&Xo5h(3PR1V92)!S*Ch)s__uPfZ)Lw+2`R*<fxrpsB>AokuXr_JP
zVdQLmeIG9&@8wsC&!QDQ)ze*{PlB%Q>gaM^2t74jvp_I&-ZhfcaxH1kB<(9x8L{Jb
z8*IeY)VGei?V+1o^%?Z!^NSim>8Z2a#ycC$JIY^+WIw3mt`&SKt=oOCMf$C>y`l>0
zro03xt<|<pTk3G2ijQo`taN@J7wwp?^Wt`~=5*Mx(BoA(bu!&<<5Yi|WUvBfbYDzE
zk^bYa{T`xE)pplNjBw6ndd@=@sQyaH@zEmQ)e!TMqFo|B0+!cWf{h0EMjV3ICtevn
zDiJ^^Mw^{8%Zd!KNJdtvXp4%@tJqkN*=V)P3dUj}fhvW8X{yT(1Qw{EwU0;|d5K((
zEQ=4K_0Q)fdTSYP>GWt$)LUMem9?PT*->0}%F+9x0=8<XX4Es%IA`yxI(gmlfX-4m
zR_QScv9rHM-k{<^V5MMmP^oLhzz%$C0BO9vFI~9m<6!9-uIb$UDxZhL&}chl+>B1E
zohrRuH&d`E@)+&n-ODYo?5#WMXY{(Z`qDE4@!>=Dv-*r-Zo@TqiHJurF^d9$&+OoS
z4uuIX6O=EOnp_c0a{b`HoQM*JAWkkPYJR%Wjd##h8<nQKlJPD+V*N&dni!`jS|)#L
zVyb1r?yk(uMF|FXFaf`SAAKA<k<{^R)s<@Ze0#0-gu&C2ZtmCIqxUu$H9Lh4Y~6Jk
z6KCY@IlH<3PBFd3{=vMD+eb}P=9gBK^FZyJ^AAKjm8IY0)jjma=g#{vhdW=;Kc$8y
z=r%;CMo(;?eXKclm=18Szfn+%_F+_}<OemU0xQ=^327(1#$+g+Q+Q@6wWD1z{cxCC
zT3tGpYkkCNGUN4i^Tj}Wq5b<9^akbhIJOPy-TgAde9wcsydzk)2DvqR)1FY@(#o_(
zQzijwU+#<*Ho)U@WfhiN7MLHfbNwxSGEGP1`xDBM<`{{`60f)|=>BJCt;}_Efbw`o
zohDEH4ZgC|jt9vPu-F9|t1K*g4V`<toArkE<chYeJ?xoV$EvTg4)l`7x{5=(o8%}I
zZfrb!oI|e_%3G&M_RX~`zvN(X_ImG-1D--h4?_a=QqpJcW|1UzY;p^nl!vSr9<&}l
zCIas)xFD-!6t=FgrMBwrrS*=g4{tnC=Zxa*q|${frgt20=*WM!HmbMbe0j0_p4#j6
zc6|)NyPIljSx%?7SQO}9iMp3vp>Z2cvM&<3eGikB%L^V#l(Dq5m$KfZFN)pu{J^DP
zd3^r2Rr%RRs0TDklA`lW&RsK~aS2L46yM*M8l$rL?IGKSUgJytJzi2G8GaoC8&ZqB
za{TR{blQGtd)#4}9=g?O{qr`(^|mmG>j&Lg|325==DlxrFL-HsJDe>;)aT$Y+L-D(
zF!s;r3{PgLAA4QW>LjNeI^cQueE$wZi<8`esvG%2O&3-C)w#XmJLd<3vOFEfqx!-b
z^`$+B$6OV4H+?M3t7ZqEw-~fmP-5$pZG)9)0{F<1*B>pk<u8lGk^7FV!>8PNr%I)@
zCAi?vJSa=^ERb>8zD~GZ#U2OM^WPu@oD35h6KxjRzQd4-*3pejKAO7Io@t)=7TYeQ
z@{*{+gb!t;Z#cfm5+NrSCbi;T?Ym1I+2V_pV(N<XyInCIMrSSh*5I}`Z$6VWBC6z^
zlXE)q#%%1kaj~3Ir=^ic!KM{XQbgUPq#0cCHg>_YbA|$U#i2@d?_&HgR*{hDYKeCm
z1tF)avc>{l>4wSpCL*L)Arvd0eeuX5sML!4@WRqKMn`x9FEWb<bmU6+h+b&5m7kDd
z)g;}GyKAEK>g&$qXXZ_@Ej#m>X~`-6;h7PIs$pwuSG%eAn#-@N5jW2m@mhQQ+v5wj
z9L9HNx!n7ncHq{2+?M|GkA;*6LIXId_>%**9v?0kO5)!)sMl0xe!y3%vt?xoj>+_5
zO!4}KNZm2bE`(*j-->X|;45^b+vWcHscRWSH!7(g8dq$3`Os2osQXM_IW?qh)U9w&
zhO|eQ%_qv2IJHge+zUP<bBTjI!MxG}e18Aw-u*{X_ei9{;~Gn>EL*4J4DPF8bvh5H
zGVDr12?*uN!Th5IFPzhp71cVE7<OkQlFXRzQrTxQzOJ-S;hGsD9%t=a-*2~C?enE@
z^qQUhwY3wMlEpNs>RIoe#U5W-PrCLxFa47Q8k)1^nJ7x|UH7T1yN<QdTN{Fmt&R?K
z@HkOc&zmx!m!4{xe@Hd-uZWJQtk!BcN{JEAkGJD|Zg3<o4AqX@I%&D=$!)*V;^oH5
zM)&Ex#T}BE10B5+kCZMPPJe~Co!wrWGaL78Lq*q@a@TEF?>%zws253d=w2!2WuEQF
zx(=^HI6q0;W`D6;ZSm4f=W^e7;UjsU?-sj-KOVU{`!wV0nuw?P-EIa#VjbCViJdZ!
zroR<hL3OrYt_w)ZR=D16wa!kKdBU$;z<#!|XCM1pKq}jOa^ty+uiGV>6lKlVYz<!%
zCRshq%F!@Y3Do>>E`8T69Tk^&6G`=k996lgIN!N~`Qx;q(WKg`*JceP#}~_oCpI-L
zw$bMbN6cAk*0YaIG|lG5UBGh+qahH1M4Gj=yQ8)BUkwV_mJ~!EF?DFQP`SCwvs5oz
zT4AAit#w!pR1fC5BLU{6Q)^Wrt(_Ai>Y{v9Zngc5S|0Bfap1<y?LHztmMa$^yTe00
z*hgd+bL4ikaY7#-ikMT2n1mE>C7yd^D1J5%D0z-TW%|kpRll-9SmoL^wK+!{9(QhL
z?S0aE>tTaV&y3mJ_^Yi<ZK1ueX~{mRR8Ap>*>~S~a#hmyEgs1|DfW`}&}of_K+cHK
zY83?zWzgZwSaNF{u{hzS0>#!^>jX4I=bYVVt9Bq^R)JdUMM&Q>vwAseXh!xSO&XRV
zya6UAgVr0g5K}mHDX=8hp*N<x)ton<IeN6SR=_SJGOO5%xxm{u)qpoJHyhIyCRuz<
zrSFpzi1j_`SNndFG|Vl6e5dl9g1&TN{oiDM2=q)<5h$kfp4XRm?rDB0eQ&O0bK<B&
zOrDk5ipHYejX>-P{K=q!_f;1T^@%`Te6<++<E;I!T09C;o16%E|9-?E{d)|gZX>gG
zN@jNk*h+q*fi2@U7iSzPgsx8@hY$gM4m}iXDIpLOb51CM6a=uLM1V?Tn8GF-nqW{G
z*%ao3bwRp>S_1(zhbSiC8RhCliV7kblVRp&;wBs%2tWte1Sp3d%wXX-rm!Vk9607r
zBVf=a5q6L%Y@3Ta)H;L-Kr#9leI(qLLkma4%*3H4Ofm)MVPp3b0(>)t1+dwnI0Pah
zB0@jHKtF^@MWBq0jS)yR0*!`)5^z=|gH7PT87vJx#19M`fJI``LfNzs29%FUAclmo
zO<^!_9r~Al^iUU<zu_6IpDciUAUK3j1WF%?pwkh*YOvV0;ULIQhyJ4m%M0wh5gq_5
zB#cP{Y{LNtTjN&<GU;#q&@g83QaEH10tg1^pePHpiu%o@y`zi!-x_=hs5E-$k`_qz
zZ<1^p<sY(s^Nqi<6wa@Xfa-ta{wDob>`TI+l#2__CWI8mchAwr6vnrYBZrV^WZcq_
z2oR7&07-xw5zr_&#sG<h6G;XbxFHrv0x%d98bcxc0_DhHu?Y+kz=s0C^=Tju21P*{
zf^u*&mVks~&}aZoAfm8v6p?~5GBQM?(In(A5N=EwSe1m}U%lc(kwGW|nqoi(up8kd
zV-yCCF~XAJ#sq)_HzZ(CND>)EHZ&$JL6J!~yAURw0H%{hCr|-ID1*ARz$YAM>F#I>
zL+k&rx-{b+Okh($1yh(ajS<HAN7Rc(2RzvXKAWhG=#3~Octc{47()Z}KSEmpCJU@Y
zJ|+sOZ}0<`pB5Y#3`i`2U#B3zk_H$H&YB4j*da`>kdR<g7~cUXUvsIvp(a0);y`18
z5|R9h|5@{%!0sP!KR$tA+R_pfx>U9}0_lel79kuUFBt;$ek_p!2n;F!e&0U}>aTg)
ze@GUJh{a&ZMuu>#0TN6W#Q@AV1|Yx*h6Wqa21H|u5s~;iIxB?2jvz1rODY&C7!6oJ
zOVL1emZ;SIT{<EF;PZq;W8g?*INHDqwGoFz;|#Ft!8;NOL;Uwcq@d6wzz9Qv8xi>>
zjm4tiMr48^oJheS0RyBVicF^b?)3jPL?av$g+n2K577j{-?;utstMwMrTvG%FFPGb
z&5t>72L(4|#NRvePtN$2@?ZS@EVuun1rYj=k$=VSAG-d~^{*KCSHgd)>knQ3ih+M6
z{HMD9-{=zm=S&AMz(y|uJh5d(wnc!az2(Hs_BKoBKFDY5hJD~jEYx8;3;dTsgMSHd
zO3lN;NfEZAi>=5jvDGUD#8-W^-3U%8v2C}ptwZSiqXA^8V+F|22pT&8%I{cxVx?C@
zAcC!qHkMwTq4x#&5Lrvb#G-^1#*P((Tb0{#IZlmJ?f89lIZvZrs^4o+c5%yUZK=UG
z>fq@@z6uoTb;rT@_zed6>oOyrnq)!*khfYZ;?LwX@w|8V9q2MkitkSmJ&vBD-fU<b
zQ&P=)_oDAj*B-O!RP}<X*-vFF)+5wEJ7=0gWMwHfe$8DnrdD>6uUDao?eUvkKgL~o
zjikD^qy-U=-rW3^lD!-*ctCh>OV(jA5|Q}jpoWH30`8D8FIUNPd4U1%=wX?)*0qs2
zvcR&ly`y4DCgWPG7YN-JVNbQ7Cv8&1y?2<r6b?C#UaJk$_Q;PAP<Z8j+A^OcD=Yha
zxY13qA~mffL{RXyXjtCnS#H(H<bG?hV&=NZ@dZk=@T$ZDnK$@px$ldO5s}s%$ta(A
zNpJ6x$4y7Zt~nJNsz$}{++D_n0gvurCb7za>zP`dl$tjOv%J+yrYiLAhdq5+yyBwX
zec-9VjU4Zi#Kz4JAXQG#x03hIE~}ZapgSFw*qJHoaN<T@mc@+gy;I9-IyI%7&ZDjd
zX&rW1N71$HmDe414&QzRAJ=fs;n61nuAYapNt><h!E%vDT!+&<!}T7{V|T~O9i1n`
z@n_1cn&}03X>r3ith^xOMZcFKr`O5dh_l7XU59hz$H-r}QB>=^IKxA%GB$s!U<FS*
zyIJh2nAUk|hLc#q{(N^qDsNaXmNn&)-K;xr=)_am>#5}~a6Lu#tYFXSWr5AJda-&_
zCQc`1_o`~Si(Kc*o?X`CaUi+eZz^fV?7>yzi!)E3$y<M^zcD%YRgE6ic)D?(t7+Dp
zv-(?%MUM8KnEhw+v3I^|ocq3iF(@y8iX(iYeQf!Yz95n7LL;%0%@MqqxUR=N>i1wQ
z?zd%!a_7ZjZl@ISM&_Tdn~%62C8qXao(EJdgJ&!(qt3aBhF3S4bezN9;|{#!e<hB#
Lt~OjN{{#O6T-;0$

literal 0
HcmV?d00001

diff --git a/panther_lights/animations/lynx/triangle01_red.png b/panther_lights/animations/lynx/triangle01_red.png
new file mode 100644
index 0000000000000000000000000000000000000000..7cfd0ce849b28dcef13cc745fad53aa824895077
GIT binary patch
literal 7144
zcmeHMdpML^+n<RN3K2R;(@5K)F-K!&oX^BaC^=>h&oIW!m@ydTSc!<7r8ZJ2$LK&#
zNm1IhqZ3LJwonv_MAG|=+I!dcz5nd%d*AP`=DMDl=UMB%f9t+~>$jeDuWJrFI#{oi
zS|<g8Kvvq?SU7>dI^gBGd>Qz?$~o>c1hU*~ugh+M6D1TD$mcSc0RT)876`xqA(H`t
z2)oO(*;(49D=oiZ6IMXwU}zPit~)C*^8t5&J@ub@&%B=;>o}Wm%H-h#qEr2JMHFX7
z*64x_PqQ@BJz+TAzvKdsvheOz7kA>-+*Kpq^Y$CV4R`;Dl*^H~3hU_<wj+mXWo9C-
z%1X9Rw<qN(7w|Hp4E^5Zjpx7r+>7e&O5#XwHgNdxKC-{pW`Mn8Z_m+Zr<};fo+qxb
zwuak3t+=*%ot=)U!Zppk9Zt2?BkA((VJn`iby^=$g+`DK#`Epe@Hf>>mtTtS*>z{u
z?%^8kYdcylpyIup`{$~(CsZ}iP`l9x$M+RJjjcnjM;0b#4}0X9f3S_4I@HaYPtzNU
z%GawX)nDmd<W{@!_Ah0P4Yyvj#m)DO%$j%)x5y^m8XY_u_OQ9vyuY`<cVBnoueTDj
zq%LS&ao+BFUb}Nz;pS-h4c7=By`0Bu%&eFlk|64Sj%MbLu7f`rPr0!IF;;n=cQEF#
z=J}!{Xc^^~$8jblJWtz{HodW;!P}d4PaV;YOKZtS^pmQ+Ta$buNycJ!G@naN+EKHk
zk#&LN&4qFeh68lv#&?wp+ToA|Y~>1z8;u8!S3r!f?J<^cg<cPjeHvIf`&QLsN+nV~
z+tWl^8vDnAY184W&`kp)N6@AwP1!Kw>a7pg=!!34LLQ?w;BUz+*)&W*W45F&Aygi?
z6lUFAg&X>K%xb4P@i!-n>&ukHc5Ta<e?T8}yqo6SR!hH^#p2k!r+90+CmZM~l8;vv
zl@QCj?YFvBQ~eno?nygS#y$I)Rqia7O81l6cOf)WmSM=N;$iQ)kkO85yiI<pl`1e&
zxx@Md@6N%#OZK_=(E#nOPRzngSGT%=jt+crHE=FB^y%yBH+9dXs{$W=7@dhSLG%>e
zlrFyHxTgqRI@beNA^s+Tl}prw)a+jupZY`s%iUBcOwm8K<EW2i@|ql30#>EP1XV8g
zUIsbF8C2OZ?-3Z}ugrP(#j<U~NMw9ZXisEZx4}-TXS|`GaR2M2AKryinzjwGbsu{d
zP%E?iwZpdh&#hA<u^zZX52Z{NpEXtyQ>qBKP(>Qhq14uqcr93Y7dwwNs>5<yCys~s
z_l#o-`|jeNN6r*ic`!YEi|QYf?AlJ(RS~bJ$p@UW4N$KxeC+jBxcTba`=?jk(OKBN
zk(HFabN2CU*GQv-=jn@TwrSeuvY+~P_cI-e-EOR&E*(M?l?Pfpe_p7VSoqL+O>J^o
z*Vv|x`(%7fC$IP3)ecYka5YI~gqa+WC8^I_%S!IQaQnkZko}wMm#5J+`Cb{eC9(Go
zrPibeh^P4lyO9QB@4rfoOnN)ovNm*`D{fAU*`B&Jt*|9(P5<cA&=Rj-hysa@>SG&+
znzldKeh)K`xtW?9Eq*FB>-4^s$rL9a3x%;~b}4<B;j_>idwTtY&cCJF9v-gAdZdNu
ze>!q-IU}}P!OD~2P%{TfntWIPfM=>0PN-sKmN%qGu~(gG*}mJXXKpN~S4Km*kzwfO
zx=X3WvOscn*a2sKKt2BEfN$ZJ;Dd%R^7!-S>a&#*Q)Vw?KQ(As?ItBn9vP&@y)4(q
zWw4#Zp4BKNUod1YA=-tVQPf#T)hO*`CCTX7Y~c^o!dMkq;)-%MGTE!tTG~+RovFX<
z#h9kv-Q}pC8ymSN1-e?^wCJfz+J{8X+m<<lG)ZSYNrIKPO`FTwl8;+iLzYB8L$OMk
zqh%cfQ=4BH@L6Pid0`psXo#6H{MhIt%c(hO&vA`f3Hy9$v-O`*WJ2*l@2i4m+P{wE
z2EZ(3I&AU;m+`9`5;%{#F2B;xTe`#XuC>Dl_c*#ccK3nFt-|#=)?ss&m3I@pl1~U~
z%4@BqIJV8QMVSPJ>4~Xu?BFZK<cRb`e2UoYY551O4WG_$B8qu-S&heD&izAz`b23J
zGxALpc0k_We2tas0&$1*BmIt*<J~D?4g2+1O}6H~m!eN4I4*yVT%D3LhuIi+Pm_KL
zzxe<Mx$$P(;DgkaD|5WDs+`QQ9CF%|nX*r5g7MS(%JK^Kr`X*-Ru_`47C1n4N7b({
z>|K6|G%2(G_O(^Jw6A(t_9|O)<Q*jnciZMZA4JGig&+Jpve!|rtt8^Um_k=v35(6E
zE(nw|+)<M`v?`>|9gm(Z9E&n}sjtzV3b*M{e=;a@KJ|-bd`QG*HTy!Z-S)cOUrx(u
z)TLsBla47}Tz2BnPN$;u<P8+HYf_qL8r!?CC~66ZzmVOJdS&iEayn<u)Zy2&F&K~5
zJ3&byQ<W+K(LJRsqN;u%?cBK3%*M^{*hieU=WiI5|NVppgisZ`YllDH#kzal?`L%K
z6BN?#sjY2k%+9)Wzr-<4`ekNW&|urEs@uf@g4nGlwq;}ZWxdW_)0Lwm_Gf}fEScJY
z1NFD-Cn}crY_mahK54|heQiJ1x^n54&9do_p+~irKm`s7i8{yQo;y~Gsq>9P+{trq
zEzr;{(UZhfBVB`Hsk095Rrst#N8rG#p47;aHKY{juunGXYlzK9-8amjH=Ro|&DdeS
z;wsi5Qata|=L4Mu4vBuZ4*MAm_|?>$j35*x7vMB6j#Rz9`Bt_xUEXGI^W}Yru@?^9
z$#?UA!2FHPrfR3#Ll=av8w2LEXM(@n8_a(@)_djb@pqlCrgkM%Z{N3#7>BYPvC(|Z
zXZU%N<fB_EI}YybvEx0y7fk20((bWNsD7AsB4FvR1$N!6vsP-ZVY}OIU7T$l>21p0
z;B%k$IcNXA=j+RdHx0p^Uy%2`h*?pSwFi6ex62vgO7~Yf=f*9x)CPL)doNwu^I|}G
zea*F-$R0j7GB>2CE=tTJZCjmXz<n7A1bT>RZtiGnZvJPX0hSlPMx+|rG@Gd0+~ZuR
zl_{e%-?Yg*_##XT?%*B^chRUaE0x)j9l4ymE^W24_4TU0zE*0-^_y<460W9l^AKK0
zppzg~{!8|1_ZDGb&(Y8s)zEQB!7ggaQ@m8sap24V8lCR7N~~hU0%>;4>RO9^1U|>U
zDJcATb8A<<M*F1E%<#xA{uZ%t_`4O4RwfE_h5SbkiQ@`!ZabXf+Yei>sDr&z>jJW0
z5R_Gvgp4OP=Z4~&Tc`!GFO?XU=IVK{WQ`K5S+iR}?0Y5ODi=!9zDea}LEV$`T}&Bx
z^3ttv$yFGwCnl0gr!M=SIcD=9^7m%rzK`jHX}?uLt&+ns3e5QPeNhwj9#~_ep)E^k
zCZ7`7Q7fP^UMIb)-i*_pozI6X`1WmM&E3`Yd7VDx@0_RtEnxQz)D{1>uj!@C{h2d%
zhX!pTkDD3EHsn9h17h+#PO>`QlwUadNCHOoQs+b;F!!l3dCF29f9v<=%?oVOLL{VC
zkKa78ikA$5$n-J6(r-7}o<!rabtrT$70?l~1HsY|0x>Wa22yA&KmelxzD$lGe7yb|
z9LA&@!d-RAC~}}V;K#Jt%LknII=ImGvS>s)+}KFUKu7`s*nofn6S4z1K_sCed=Zxf
z-iw-%aM+@WfMp2ZO?HHtbNK)ar-Rc$AuNT=5DeT%3TD8kGe}MrR^K4NCquZOKoCem
zB11z%bwaT^T)r<7O(YVLC=3#VL4X#BpfHYrB1CY4)I<<pF)V-}8lM>`U~)My5hjJo
z4Hg)};b1@P&-mDZWb$9|oS<(kfP5f@lt3g}2Zdy_k>7g+2`ocEkZ%F~TaO?Yu!2T9
z0YThgJ`J!80XPD+?;+^4zx)G(`2maR&}m2@0APcrLExz9ABMEHB|HA*A)>&S$qrof
z0?GctQov;VP1X;wiFy{(`92WP{V&`ftpCh?(HOKMlSvj_TCgZQTMI+DXnYc#OJmYW
zi?=uwor1>V=m<2G3LtO{Ivzm<uv7#NO~DZK04kn_#eD~5%Lx)tI5a>61%m4^K^zQ*
zp~t}KVGwiz9fTl&-V`F1fzYKeXcQ`j!eG#V?;y7GnV>2u0pCX@f}(>^6e5F8$AY@R
zQ2-hOM`REYR1^w>zz~U4Dh*3SV=350C_0T~#pSap;B+$C6kh-t$njn55D`u?b+k2v
zV{}k|w>Sn+1Psu@5N^-p1PlM}aAC3mX8}dTCRz`pheqjP@B|bZizgEPHrfU7gFq#U
zFwrO->{ndTw2;7LKw>E(oq_<19$+pcb3Q;3aQQA=Zh#?N6cSA2xhQX#!PiN#VFrN~
zVIswU)4Vgl`}*{C2?Q_~yI`<I*^(%<uY&|pLIC>WK%n2(E}9>O;|qZ6`<tNtY-j$P
z$-+^w;G|L*2qIcnG|N~lf{I1q5f~H>&?QnCx>Vf%Ko8<F1fdi@VCoBI3T6WeXfYd@
z#v+xPKgJU32Z(q=VQ>f(5rHPUpm8KDj)W#^qc9{C3Xc5mr-%Uv7=kX2hCt)-6a)^7
zCm@J=3=9I)GM-8$VgZ0g{4wbNYl^zy8X%!?|9>g!F;H}@E|!AO)x!}GI20NOE;$Ak
zfyU5@cs(2qOULQ{=TgLAUC;y)3Qa;0eoWB-DS9{msapf&|5El>gYRDE;Ou{G12+k9
z&q4mR>3oxgXfga3Ki^i=e{lp5`sX13h~J-d{iN$3G4PL!f2!*zUH^!Ie`NeqUH>t<
zr2am(0vxcG4FwOS^^h7Kc%WTIwX?QZJlaBL&FiDUJIO#Bw;=Ej5jD{T6&4zYfQ=FY
zTe79Zh@`R{bou8`o7aO)YXz3O1?F6~==1|wEXn~oER-qmgNcfA*BBXP2xJM<*22_9
z*!|`N+3pX=)rayQtDQ<tkTzAuF2`Qt=|7u&sq=YL-P&b}!A2_O&gHy*$(Dytw$yGM
zHd#_YK7R0AwjMW6nhnh$cv=y>=J#`nOS27qHmni5l4muKt&|;u<XP=$gq~}1W1ovl
z%^q*6=Gey^RuIlE?}0{5O+#xYA&nK=Pg?gCSG!+x1Ee)pN0v0rNO6gni&EhbndmP~
zD<?c?@7qxgWg{oZKC&OhJ+;bSHrN%kf7bO;{#eqa;8i~4_4>xeg4CSO=<(nxrM1Pq
ztLrX2O$lXvURV|`Ue-4FdCA8@z1L3$;=B4w8=@XOSSanw(sDX39^v(pwZe0KnN7}>
zOLsmU>Q4v|--Gr}eK=*h^o2`m?R%Z^h?*jJ&``vtj|~ZzQ$pPY(L2>s0GW$L;upta
zIx|ix)~Kte;3YhqmN572S5!~Y`Nhp~e_LN|rb%7iF%QWz?Y|K-PwEME)KeOjZx@Qo
z55S2TCT3n5Rr2CXw-<}cV?s4EP0U`rDb6r~<95oc$1hX$kUSNQJS`rfjaergOE@B<
zS_(hCzqRc4NZ$o5jHK{Lg1waLMR<BlT%NA_QduJF&JtB>XNHQw<1$TYVZ56&exh+(
z%7%tgdHT}v)8bFB*K5T{PUvj&k!nbiKL+KNil1$IrV(R10c13(eB8Hf;!K!wX<uT7
z4}ER8ckeF^3-I9A&~W^C+}>uT!UJDyGS;Q*_o;mBU#ilVbH%tISj_vn>z*b>q2h^$
zY99l<WctKDLX4ti`^1C~Ew@$lwR>OR*s%Z0lvL!H;(UvykT>|a_2r$>c<k@d4|R}9
WPE5~@Yj{UQTDG-xusCn#v;SYk{!at|

literal 0
HcmV?d00001

diff --git a/panther_lights/animations/lynx/triangle01_yellow.png b/panther_lights/animations/lynx/triangle01_yellow.png
new file mode 100644
index 0000000000000000000000000000000000000000..5a1efd5776ba174c73592e2fd6191379a5643179
GIT binary patch
literal 7601
zcmeHMc{r5q+a9}7mLyA&X^OIpS&VHggRwKnPAM~EFc>s582b`J*+NQGiY!?wBx|G;
zvXxL|D@9S%n<Y!%L%r`?$M^f^J-+Yv-#f?gJo7yFbzSFuU*~<^_i<mxlVE9X$jh^v
z2LuA~8XM_Z0sjiXb(EVExbLt&wGIODbOqZuFsulH5I>r)8^xOpVFdY+A!H`S4FqDo
zoICFr@TQJ8Zmc#@K8hX8*Ezv74@jMQcQU`^&Z|YdJJZazM(T#c0q!+6kDBibORqQD
z<V?C0D8;o?vq1&tvKc-l_m02*CKg&3ehmJi$9>`pz3|J5HdCm!ZFNw(=Svs;W9yfY
z5Mgk|LPf$cl{|XkJ+0~WAT2GdWKWFMJ@Y^G1X>$!AMX*llyFsJN5SgSieAPM*yTHS
zE|nqY!)+qgEp8bNh?`8h3m!M|8L?j|PVFH_U6PsW?#u06Iyv~MB2U-sHYq+|t!-IP
zY%U@(*XwoRgK1GwV&mS}5AJn>pJb%=2~a|W-*4nTNvU1Qye4@p;x?=Y`e-~=%VsWY
zJ^JGNdyl$SKSs6Gdn#ciAM}vFu7s^G2K5f=Rq#)SNUR9;g;uQ11SRr+T$eMH><H^F
zdG``sKP{7fdd>TNb>@fU6R)#7nlgh#E=v@ZjSST<lsBz{ub;bIs(RN~r)V84Q`*}K
zUajj!fRXLz?%TRz5(ZmM(z2gyV{adwFk;!i&BG{1B+e=$P=3Pcj&o<1<vJ~#BhJL-
zcC-D)>zdVPQSU^vbmzZzTQ|O!Ycsz(AN6@SmSA%r=U{QL)biJjZ@23VWQ**x<@U^m
z-wjF3n-IA>X*>N`dn&k6E90w~cA|q5SXxhZoF2a$wPRPBAZtRYP|;3|cZLo}bcuX)
zh7jqx>dlkYE9T%q%fq#j*GiDJMaW0GR2Sq!6XO;O<6YbRld^4;y!uM;A;~Y!XVKfG
zdm0~{O+0thq`185saI*SXIFK<(~&v6lU`GG%QL$pUX7I=?r*zm)Y*b1QIkeE!@cDG
zzLDvj%TLh$Zynuosf$u_3fxVfWp9+<`Np^B&|H=OJLiB`)FrllbB&ql_QJc?7V7KH
z)$_fJA6CbToR}rj_66Q9LH0?71r67^r4M>4T_c`l<$&8HC9-(mxKOcA>Q;wi*Zh8m
zJl#~!hmI|>1{}}$t{!agTC(X*d-&b>)zM=6S8wi><y1ahCrpJ`3<ugCc`Nv(WC)Kl
zElIhV(i1pmYIqz8IS2}B3tn6ZOD=ti?2s={4@Vug5^s{?&CkEx*Ny!$g{{BTkUMqn
zdh)Vqw||J<_V)Rv2Z{s6F=HZ9s)jFz_BcC-_TS$de8>u`Szrah+_wuUo+ZpEkTNmm
z)RSlO28stL>5F*_v&ZgVFqtw<&wZp3VnGuTdgj&6d+(bW$1c0WujIfqiPK|!S+WJ?
zhnH9cNEW3Rkp1d*raG1{W97sK)b0*;Ee+mo)8(KSRn8h5Hl=FAhTO6xi`-JGlbP48
z$)&QQhjdc+OR2f1bSrS%)kyoLzh}u0%W98fPdXApMF|@gU2v{B=d4j(KABaPX2aQ3
zwIGfPL|nd*$@v7r+h5!ex64$GoN|F>Qg9%3gt(orSpbI>auM@-8*?PLhX&qOTBA&0
zyyR&1&dA(e_NvJ7vs&2Wm{k#(Y@QiZ74$xs=V^{V<&qFz!m-NL+_jH0cbiPEX_1<3
zVSc8&t{ABcuVrr&n25}Occ^NgT$$|9?!xJr{j)F^xBJzI6fEQ-`;&%=ZHT4)5-$?U
zPWGntUC;Dy>1TUuc`CblR|+Jl`~WsudkFhk=!$3AL)R9?4BZsjXU*w|c|og31}~Fx
zm7XO**+0F09>x?f2QN*Z9P(aF%?N7y>|go)O!RW>O?+xYGj=7J+ca0AkuI;@I3@HT
zC*n3ie^kxlslfZVX?KUD<g;bD*4ek_cyQ7~qRwsJ#}2a<>O(WfCQ8MU;tdYCq(V$P
z1i&fyLaL6CrQFeTnj+3DYN+<)6RtQeXX=h3gFEV&<neM1zNCuXS?`@%I+8Tb6VKm@
z$dWWYy$XZ(L}C_Vc5v#~KvriR_s0m}^=Jc$7zgjg-H-E_9sDWU+!CKM`s*@_dHj<P
zkgk086JB|h-*6nG9~`$&4<(pVOxR86nj8{(Ix<*(dW<Z>&L^j`UTkR|*b7VW@%9u~
z*Y2|IZrgdc^1w29WU`%au7~om|8Q(%)rJ0Kr`O{35b|TjoJ82*=)hAo$)K*|GjDvf
zUWC11x!ity#lTqr!k+G$p<f%QcTioosv6pv{~5F|(C&lfjw2k45+cLbxSPwx>DRgV
zre8RLG<i#HQZGU5%&weDWlU9(jZWKiW#PWL$%M&Sg~zm2%S(?gXW7tmF}i1H7i_gN
z6_4ff5A2AJTo1@f5U@blPm7`KZg<NBW;O3cofs@n3CfKB;)D5A;i~9U>#Ore)5~yz
z$c+BAo({{{kNYa)Bsuq)aLQp=qbtN9LpHdFCq&I)F>-(G%AV2ksYF0qLAi#Va<2>W
zyvKF!@E?%a7De~)H@4b-w@pqtQEE!5>+CpmCGN)ZVM$f7d{0MKy@KJ1ocn0yUG3b-
z*Kn~R=+SyV4~VskfYVY`POM{$G-qMsypF}1aSS%PZT-r?z#iY^HB{Pi(3Jp;{r)tM
zju>HgKs3egq@+XY-S85-OvGh5P|$jHVpA1tHs5y)AAOplc)iQ!oX?bK=t~c+MHgzb
z!CH+xO)JL7GtR8iCCs6<n{O!E;E%&*v3mFSxv_>zYVLi_D#1`ndL3Mljkb^%>th31
z`w5TWyoDro!QLdLt=TiajSfP0?>r&@h7a5=z5Vnzomk@!0WK)Tvr1ogIyb9_#a6`=
zCB>j*d=V&)yOW<&eJ77bGQ>I$+At)+sxF#TnjZXZtS#>91q=TH)30cU$a3=iVixSa
zSpkNVG-KuwCiPO<!9%5HvVB4nY$E$G=P3Uv+$3MC5vtEwyyg}IS`#1OlE<^_h++yA
z3$7eh{fspU?l@RsU@<feo3=(*sxVGjaS`R;8ZIlSlz+^K7Yf0mn@oJoGN&&d=4KS_
zQEHG7>djyi-{x$(oA}E5;9ZYXjfCBXJEJc8d|Qq_c(!wh^6A!`hvt*#%gG#}GdvU9
z22$QnATHS6;QP`IY~nQ9wq@u8BaJ&Mglx44Idv!B@!{+vk;+XNqjSYyFT}flE5ij~
z@AT$wJUJf0jS^D!Vtev6R(#fz?}3a<f$<U69nhou-5lHsC7n`Q2;$FE>u_O5&qVK&
zFiQN&8R|9{^_cI5E&oI!@zMu*F3W}cabg+$Gv{5?SW-c?lXY?u6q)e%Go$uP^`Q=n
z(GK_TNIaiMI2oWqeX}_~zGo}>9_oKwaxMI!_B7P+W*O`0gg)VtZC{kHt1GfLF8YOb
z&ph6EiK~b^wiN3~a&K)7y^}yYK1QBZdafy|*%&u`dE}<j^}u^Q{IKGb^%GRqnOeV~
zCJ?P0{MNDf%CiriGQF|5)*gXJeRxODmfFf0J&!%X?R%@<VqZS>GW^(Busj>@=6zEl
zXEAa7*ryM)cYO2Uo`&Wfc@Iw|qQy5ZR)uzt=cWewJ@^*$t_k1n`$4l)bK=Dg+X~qC
zj`&q+DQxLVR_5@>Tv7A9#on6G+}##OqYKZZ?;+q1^N}CKPv7d|sM*63nD^yfDZ7J2
zsrgBRartD=;pE}6eB^_TD>tW$L@JNbhvUj?6BeG$awhbT@NWyv!8S-gdafw6hI*)Z
zvOayWD^9hzmo#8ETNUOXEI0DbmmMq0${>tXG{1^S&q^WGOTOMf=q&HEkUSY6d38hU
z?e~MLrnh-h96GTqQRX)^dK@4!AP`F&1&6aV#^L@v*aFAdoUk-aqgHM4hsUi8WHb4M
z*P8d?{BJ{Kq2^ApP@6;5x;OZwv%<OYyUz-V7`9Z8k3S+lZ+Ylw&t<P8um(C2;Ah21
z6a1bf<n(~)H<A*#v^Ve*D9?^q@<N%XD4U!=iA1J5?_jH#)`RPwH@Nq}EKK==Su;KK
zRqLa{`a?aRwU*vZ+tH-iLZJ)WpYfh%<}zu|o~nJ?dCKvyReVo^;r2Sn!u~;W)|je@
zxG>Xg$f)pbeCq>ZUhI^xn?6qJ5+wOhiNT6)8#(riuzR%)At~gu2q*pJXTd=VA2gXm
z9tz%pk{!|p3tz4B%s+3`7yhIbJHDL$=4^X4i$QWwMxHKhZ9HPW{)i%0>#Z~gNqhA)
zHDWtUl=GEi)pMUn!{wJj8}8#KUf(*ET;8Yu;c0zZoF$JsK3P}X9@0F;*SnN&8u!L1
zJX=?bzu|J93UJUqa>eWUT-mLZXIv1xvy@NdNu0}F?H68qKYjF=n;TP1+6V{Ls?b{J
zcbrHDN|1F5P--~f%`hZissh2)mq=D%QvHBZ1O(E+GW`f7FERr{B)e05G@+mB??E9H
zS52tB5*~r~!;w8GM!__)b+EY&DcFmo<_g7X@n|qH005QDAV8Q@Zy!2_sR`Y}#Q@Kn
z&2T7W%Z1^k33b3*LU6t`G6b!FRzSe?nUnw&REr0qL34G(Sm_!3gaF<&p&kr|9|jH&
z3=C8VR8;V#xx<laYHDx<3XVd-01p^F$cI5-!hGoaHz9st=#l9p8pV%6@%4dhViJhH
z{tQhh6tF}793RyWkN*qahyIfVfDbs6;0H%4AmCIg{MQINLq7lj`8lBf7(urIDmJ(k
zneOXPBa!t3$Ucnyze2c@{)+eWr+IIs<4S^)y~$L-l@9cZ{H;qvW4z^G5t|gaQ>cDh
zQ2^P$c`_(&f0Omw*fuR&>HInnAp9@f-@O0Keaji}!s9V|z9j$6;Th{`LO1)zxcZVP
zu9&SyRb`TrGFb%$Q%0lRU}yq~1S29<6=7r&nS>@G39c$A)nA~Dedr8=4~e`91%NA1
z030`E1X-1UAi<DmH2^}<l>}2GyD7sI(JDj&T2WCAu>S&KL8AbwBzXTis!b?Y0E(ch
zgaSelFeM^U2?mS^0aJBVQh})uR8T}kB{Et`No5Plm4q?yrBMmMbW*4UcQV}1$9>DN
zNjOHw(pVFUQb7FOV(Cp_xB&s0P&0~;Kl5*c4TVa!W)L>nM5>@vkO&o&GFnL$sr)nT
zEj&9ijSeVr6BCJ0Q2c?rIV~6<8Gu;ArcMEXEm$BI430)7Fnnn?zP{d?&`k))X5^N<
zAsRm>#fU-&Jc2eA|5NkU<P$&Mek=iR%9aTN*^(`WK>E=Moe)5F-RcO4`(Yw^5PaOp
z!213vs6X2&|7o&R6%h!uvI-38MkE6aE4#vo%19!NOeCox(W<U)$|&XE(doW!j6ed7
ztm6)33S<KaXe%4Yp)D$9e)kUaAaC-7K%rp>H5f|K2C0HUpfHLEIpB^!K;i%W6qOWJ
z)l>)sMVJZ_MF1qNLVyuSDvB^Sq7p%sh$Ny&=pQ2bi@^VDimDg{5`#eho}vbPbL0AR
zQZ?ZJYubM}{Ib&l)cj}zc2HnLhX1uA|Kw~_DgVXaPr3aUdjO#S?c^Wv`!8Mp()EuR
z_(#tFs_S36{t*NJ$oXG&{lC%0^LI^0_5luhfk4Gpsa(1F5sH&&YN)qW_kmV$^%1}m
z*w4t34t$;3zj?7R3$Ot|BNxLMug^6N77<`!{{!!V0Gh-Y`VI`7FLkpt0BxOE$*zz<
z3c~}kd1AGX;u8UZSPP8xbZmfcS2?B@?=6L52W2jbTB5U4$F)h5r1AdZdn0*RJ$LEd
zp>iBmIqE|-S)Gk8l#sn42dPiBO@~8sN)?*YL7b7X_obqq>cg28g*$V;g<W~Z@>GZ!
zVUM1C^r+bdG&>8*Hn^;y5P5OW_+mhlb?f72Q&Kk@yY@J8niXQkXU0LxYwX94ubj}}
zjKgt^pJZ`?&41+xjp(tTQIY!^9Ese1nY>+NH?rZ)W6OG8Zz{{`YO^b=h9(OfE`BkE
zzjnUgb0NJ8e`Sn|k3Z@ZS6pUJAd1sS(1pY{^oqmWLX3%-vh^GVyZC{o1JqEC%3OPs
zn3=+29hXJf1Wve~n{sx#2ah}m?)8DcUEkQt41c>v3D1$dJT@ORYGKZ-8XFZoYNh`)
zV&JR#r(C{H`)t;fl+Utf&&@sgGCJ2$_BeCA?A&Z(zAgOC{0&)D!01e&$Z1)W+vv8I
zEZh8;2J1c+tIWs&HKVJ7y>(nB*3nkuXEP!@#N~~_MYCY5jL4Fl13F;YlaK6JOM;1M
zg3IMbC3M=8#8Te9H=xBIIf%9gYVR7INjzXL!sDD~-h#6`Q2WIBvE$C#reR!Hc0=T;
z^Ga%5{7PP*S@#kL-tdmp2r{{r3fPaegvmw)`W-hBU>`Xp$j%X1%-+)MET?1Xmt`Wr
zF%m1t&J|d|ez@8BkPdpj&O|_>p+r!Rqv9HSU&{^IDDHV#69`Yk8NqlK_zm`}&7Fs$
zjOP6_w8fX}Odx^{MqufkE#ufJ=QmxB8B5ppZ(zzi<|9_aeEC(?UsFkC$&!Ij+)hjC
zdN$LS*LaIqxt2RS2V~j5TW49;p~rcdY@?AX<8>|VP6gwY7p&$N9AuVX<yy_(kSiD;
s9K^JAt<;)J7EV~s#9v!E7<01|ev7R|2X^;M_vSh<);HHH*L8{hAN0~yhX4Qo

literal 0
HcmV?d00001

diff --git a/panther_lights/animations/battery_critical.png b/panther_lights/animations/panther/battery_critical.png
similarity index 100%
rename from panther_lights/animations/battery_critical.png
rename to panther_lights/animations/panther/battery_critical.png
diff --git a/panther_lights/animations/battery_low.png b/panther_lights/animations/panther/battery_low.png
similarity index 100%
rename from panther_lights/animations/battery_low.png
rename to panther_lights/animations/panther/battery_low.png
diff --git a/panther_lights/animations/panther/strip01_purple.png b/panther_lights/animations/panther/strip01_purple.png
new file mode 100644
index 0000000000000000000000000000000000000000..bf4df0a71b0cec1551b846e2d49f89a58654699b
GIT binary patch
literal 964
zcmeAS@N?(olHy`uVBq!ia0vp^F+jY7gAGXLS#_UbU|?*?baoE#baqxKD9TUE%t>Wn
z(3n^|(bnUzgUr$R;H9owvbP+MC_2AQaSS;UxH_PPYhhT%)q_ti>1SM;uw;h?*E&{q
zd$HAgdgpdo?ci#9z}~UCd-9_0DL?ivyP$Ne`ElKz-QRaqvvZu)y1Zszp~K}|<H=rK
zJO8YbsAz34XzEZ_GCn<H@)8r9<JWC#YW8}6(0N~6-{-hB!SH~((0QZ7TRP1<mYe4`
zf3_=}GBtJTk2w`*mT(K%7sVF->@8k-mg#)Ytea<S3a3QqeBE&A@S2>gBf&=uLoaK_
zpV+yk=-#6>p|95z`~N!>;20nI%<8P*k_{cI9s&y&^l`1)FmXYs`cbF0Z<FQzJ=_+r
zG~H<(mxQHQKr`FUx9tC$@869&dXm#za|+9^c$w$PA{=j+*0w#ak+=4H&GNyXIlSt6
zd^VHu{3YhQqTX<D<THz3kM+IDtXJN!P~F+=>9!}!HAT7){FbZ@mg~5~#GIX%Keu*{
z9Yf?Rty*Ro8K#dysq3rvSl_sPdw;ss-@goYQ;H^KuYTVTj25;eZ+90U4Fo@(ch>_c
z&H|6fVg?4jBOuH;Rhv&5D9B#o>Fdh=oQa>CLF3M~*6qNUJm~4-7*cWT?cK<{LjeLV
z7tJKjC`|dQZ^CJOlSzIpSCv&cr~A@vj*c@|y$bEx>ck+kqk&ocLw`=(E0=?Z*yDa|
z+^3;(u)K7^1eW{P`kaJng1KAYcv=RXm?y$repolA_dzw+&Hir(4zF*G=JM-mUsiMJ
zi=3YNQBTHqOCl@^Dl?*X+++J5%k^R6!Sn~2A9!0;e(Xx)I?&Em9(3qIHkaoI(E{_D
z53Rpv*Qg&obd}AHaeD!GxcUzl#vcbBu--Y;xLS9;#(_p|ivmV=y$6_#|5yKf>R;8s
z&-*TZW28mF)+T#Rm4ovS=-xSWmGj?%gon({_OJX7dP-J3JaG7dIai)VL22Xtxl>Gj
zOm!0a!zT0N!2@xi8?+0!+1H($T(m6Vq1>#ZpgAIzn)00gycMg1YNiXZ?vL0p`O!kn
z_YcL^eLryitM5T|U-Ot+^{4Gl8!}f;p1+axbM)^ohfe!X)CGrK!2>2f>M?}=GVaTX
Vh&=Qs{S7GcJzf1=);T3K0RXWBpEm#i

literal 0
HcmV?d00001

diff --git a/panther_lights/animations/panther/strip01_red.png b/panther_lights/animations/panther/strip01_red.png
new file mode 100644
index 0000000000000000000000000000000000000000..5886cfe1022b5657090ecdd55075c2cab06b0fb1
GIT binary patch
literal 900
zcmeAS@N?(olHy`uVBq!ia0vp^F+jY7gAGXLS#_UbU|?*?baoE#baqxKD9TUE%t>Wn
z(3n^|(bnUzgUr$R;H9owvbP+MC_2AQaSS;UxH_PPYhhT%)q_ti>1SM;uw;h?*E&{q
zd$HAgdgpdo?ci#9z}~UCd-9_0DL?ivyP$Ne`ElKz-QRaqvvZu)y1Zszp~K}|<H=rK
zJO8YbsAz34XzEZ_GCn<H@)8r9<JWC#YW8}6(0N~6-{-hB!SH~((0QZ7TRP1<mYe4`
zf3_=}GBtJTk2w`*mT(K%7sVF->@8k-mg#)Ytea<S3a3QqeBE&A@S2>gBf&=uLoaK_
zpV+yk=-#6>p|95z`~N!>;20nI%<8P*k_{cI9s&y&^l`1)FmXYs`cbF0Z<FQzJ=_+r
zG~H<(mxQHQKr`FUx9tC$@869&dXm#za|+9^c$w$PA{=j+*0w#ak+=4H&GNyXIlSt6
zd^VHu{3YhQqTX<D<THz3kM+IDtXJN!P~F+=>9!}!HAT7){FbZ@mg~5~#GIX%Keu*{
z9Yf?Rty*Ro8K#dysq3rvSl_sPdw;ss-@goYQ;H^KuYTVTj25;eZ+91l|3DUlv3kvC
zAjMhW5n0T@z;^_M8K-LVNdpDhOFVsD*`G7<a~Wx`%zfnzjLAk%7srr_TW{}1avgFI
zaJ|@dZ03W1;#qUrb-%a=m$es9FQ}N}$ipZ2k$34<CkB}v4b0*n)TGtf#P+>toiF*M
zNgjybgRsPtruh;PJEZlv-;4b@!0=<y!R<UcciNXVRP#-5Wu87c>EQuZnH^mRYo{~G
z?9gh;2deMqX|UE@#lA0%;SO)OZNdH%<v}_z2O7C6W<OwNegIbfX#!9+(9VJfOnexO
zf9u`qgMo%NFo!GtXlDefxu^SMc4Mo))SsgVJQ>$jJ@9ACvna6CyxU|i!N3mWm$QM4
z-~PayO^h$bud9{2V)BD<ww(|5d2cLu@M+;o%NVhHYCl>VTX#QP#cpTpkvl_a&xvU>
r%HM7lmlMka`vB}1klU!jn0k@lcD264o%Ls~fg;b-)z4*}Q$iB}%Kv?@

literal 0
HcmV?d00001

diff --git a/panther_lights/animations/triangle01_blue.png b/panther_lights/animations/panther/triangle01_blue.png
similarity index 100%
rename from panther_lights/animations/triangle01_blue.png
rename to panther_lights/animations/panther/triangle01_blue.png
diff --git a/panther_lights/animations/triangle01_cyan.png b/panther_lights/animations/panther/triangle01_cyan.png
similarity index 100%
rename from panther_lights/animations/triangle01_cyan.png
rename to panther_lights/animations/panther/triangle01_cyan.png
diff --git a/panther_lights/animations/triangle01_green.png b/panther_lights/animations/panther/triangle01_green.png
similarity index 100%
rename from panther_lights/animations/triangle01_green.png
rename to panther_lights/animations/panther/triangle01_green.png
diff --git a/panther_lights/animations/triangle01_orange.png b/panther_lights/animations/panther/triangle01_orange.png
similarity index 100%
rename from panther_lights/animations/triangle01_orange.png
rename to panther_lights/animations/panther/triangle01_orange.png
diff --git a/panther_lights/animations/triangle01_purple.png b/panther_lights/animations/panther/triangle01_purple.png
similarity index 100%
rename from panther_lights/animations/triangle01_purple.png
rename to panther_lights/animations/panther/triangle01_purple.png
diff --git a/panther_lights/animations/triangle01_red.png b/panther_lights/animations/panther/triangle01_red.png
similarity index 100%
rename from panther_lights/animations/triangle01_red.png
rename to panther_lights/animations/panther/triangle01_red.png
diff --git a/panther_lights/animations/triangle01_yellow.png b/panther_lights/animations/panther/triangle01_yellow.png
similarity index 100%
rename from panther_lights/animations/triangle01_yellow.png
rename to panther_lights/animations/panther/triangle01_yellow.png
diff --git a/panther_lights/config/lynx_animations.yaml b/panther_lights/config/lynx_animations.yaml
new file mode 100644
index 00000000..87252652
--- /dev/null
+++ b/panther_lights/config/lynx_animations.yaml
@@ -0,0 +1,126 @@
+panels:
+  - channel: 1
+    number_of_leds: 22
+  - channel: 2
+    number_of_leds: 22
+
+segments:
+  - name: fl
+    channel: 1
+    led_range: 21-11
+  - name: rl
+    channel: 1
+    led_range: 0-10
+  - name: fr
+    channel: 2
+    led_range: 21-11
+  - name: rr
+    channel: 2
+    led_range: 0-10
+
+segments_map:
+  all: [fl, fr, rl, rr]
+  front: [fl, fr]
+  rear: [rl, rr]
+  fl: [fl]
+  fr: [fr]
+  rl: [rl]
+  rr: [rr]
+
+led_animations:
+  - id: 0
+    name: E_STOP
+    priority: 3
+    animations:
+      - type: panther_lights::ImageAnimation
+        segments: all
+        animation:
+          image: $(find panther_lights)/animations/lynx/triangle01_red.png
+          duration: 2
+
+  - id: 1
+    name: READY
+    priority: 3
+    animations:
+      - type: panther_lights::ImageAnimation
+        segments: all
+        animation:
+          image: $(find panther_lights)/animations/lynx/triangle01_green.png
+          duration: 2
+
+  - id: 2
+    name: ERROR
+    priority: 1
+    animations:
+      - type: panther_lights::ImageAnimation
+        segments: all
+        animation:
+          image: $(find panther_lights)/animations/lynx/strip01_red.png
+          duration: 2
+          repeat: 2
+
+  - id: 3
+    name: MANUAL_ACTION
+    priority: 3
+    animations:
+      - type: panther_lights::ImageAnimation
+        segments: all
+        animation:
+          image: $(find panther_lights)/animations/lynx/triangle01_blue.png
+          duration: 3
+
+  - id: 4
+    name: AUTONOMOUS_ACTION
+    priority: 3
+    animations:
+      - type: panther_lights::ImageAnimation
+        segments: all
+        animation:
+          image: $(find panther_lights)/animations/lynx/triangle01_orange.png
+          duration: 3
+
+  - id: 5
+    name: GOAL_ACHIEVED
+    priority: 2
+    animations:
+      - type: panther_lights::ImageAnimation
+        segments: all
+        animation:
+          image: $(find panther_lights)/animations/lynx/strip01_purple.png
+          duration: 3
+          repeat: 2
+
+  - id: 6
+    name: LOW_BATTERY
+    priority: 2
+    animations:
+      - type: panther_lights::ImageAnimation
+        segments: all
+        animation:
+          image: $(find panther_lights)/animations/lynx/battery_low.png
+          duration: 2
+          repeat: 2
+
+  - id: 7
+    name: CRITICAL_BATTERY
+    priority: 2
+    animations:
+      - type: panther_lights::ImageAnimation
+        segments: all
+        animation:
+          image: $(find panther_lights)/animations/lynx/battery_critical.png
+          duration: 2
+          repeat: 2
+
+  # The BATTERY_STATE animation is no longer supported.
+  # Animation ID: 8 was intentionally omitted to keep numbering compatibility with ROS 1.
+
+  - id: 9
+    name: CHARGING_BATTERY
+    priority: 3
+    animations:
+      - type: panther_lights::ChargingAnimation
+        segments: all
+        animation:
+          duration: 3
+          repeat: 2
diff --git a/panther_lights/config/lynx_driver.yaml b/panther_lights/config/lynx_driver.yaml
new file mode 100644
index 00000000..576c6cbe
--- /dev/null
+++ b/panther_lights/config/lynx_driver.yaml
@@ -0,0 +1,3 @@
+# In ComposableNode ros__parameters are not supported:
+channel_1_num_led: 22
+channel_2_num_led: 22
diff --git a/panther_lights/config/led_config.yaml b/panther_lights/config/panther_animations.yaml
similarity index 76%
rename from panther_lights/config/led_config.yaml
rename to panther_lights/config/panther_animations.yaml
index 4860ad50..304640a9 100644
--- a/panther_lights/config/led_config.yaml
+++ b/panther_lights/config/panther_animations.yaml
@@ -25,7 +25,7 @@ led_animations:
       - type: panther_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/triangle01_red.png
+          image: $(find panther_lights)/animations/panther/triangle01_red.png
           duration: 2
 
   - id: 1
@@ -35,7 +35,7 @@ led_animations:
       - type: panther_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/triangle01_green.png
+          image: $(find panther_lights)/animations/panther/triangle01_green.png
           duration: 2
 
   - id: 2
@@ -45,8 +45,8 @@ led_animations:
       - type: panther_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/strip01_red.png
-          duration: 3
+          image: $(find panther_lights)/animations/panther/strip01_red.png
+          duration: 2
           repeat: 2
 
   - id: 3
@@ -56,7 +56,7 @@ led_animations:
       - type: panther_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/triangle01_blue.png
+          image: $(find panther_lights)/animations/panther/triangle01_blue.png
           duration: 3
 
   - id: 4
@@ -66,7 +66,7 @@ led_animations:
       - type: panther_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/triangle01_orange.png
+          image: $(find panther_lights)/animations/panther/triangle01_orange.png
           duration: 3
 
   - id: 5
@@ -76,7 +76,7 @@ led_animations:
       - type: panther_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/strip01_purple.png
+          image: $(find panther_lights)/animations/panther/strip01_purple.png
           duration: 3
           repeat: 2
 
@@ -87,7 +87,7 @@ led_animations:
       - type: panther_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/battery_low.png
+          image: $(find panther_lights)/animations/panther/battery_low.png
           duration: 2
           repeat: 2
 
@@ -98,7 +98,7 @@ led_animations:
       - type: panther_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/battery_critical.png
+          image: $(find panther_lights)/animations/panther/battery_critical.png
           duration: 2
           repeat: 2
 
diff --git a/panther_lights/config/panther_driver.yaml b/panther_lights/config/panther_driver.yaml
new file mode 100644
index 00000000..10b4b378
--- /dev/null
+++ b/panther_lights/config/panther_driver.yaml
@@ -0,0 +1,3 @@
+# In ComposableNode ros__parameters are not supported:
+channel_1_num_led: 46
+channel_2_num_led: 46
diff --git a/panther_lights/include/panther_lights/lights_driver_node.hpp b/panther_lights/include/panther_lights/lights_driver_node.hpp
index 094dc091..f1a4b5b5 100644
--- a/panther_lights/include/panther_lights/lights_driver_node.hpp
+++ b/panther_lights/include/panther_lights/lights_driver_node.hpp
@@ -83,7 +83,8 @@ class LightsDriverNode : public rclcpp::Node
    */
   void PanelThrottleWarnLog(const std::string panel_name, const std::string message);
 
-  int num_led_;
+  int channel_1_num_led_;
+  int channel_2_num_led_;
   double frame_timeout_;
   bool led_control_granted_;
   bool led_control_pending_;
diff --git a/panther_lights/launch/lights.launch.py b/panther_lights/launch/lights.launch.py
index eced7392..f77ef14d 100644
--- a/panther_lights/launch/lights.launch.py
+++ b/panther_lights/launch/lights.launch.py
@@ -15,6 +15,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
+
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, Shutdown
 from launch.conditions import UnlessCondition
@@ -22,6 +24,7 @@
     EnvironmentVariable,
     LaunchConfiguration,
     PathJoinSubstitution,
+    PythonExpression,
 )
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.descriptions import ComposableNode
@@ -29,12 +32,14 @@
 
 
 def generate_launch_description():
+    robot_model = LaunchConfiguration("robot_model")
+    lights_pkg = FindPackageShare("panther_lights")
+    animations_config = PythonExpression(["'", robot_model, "_animations.yaml'"])
+
     animations_config_path = LaunchConfiguration("animations_config_path")
     declare_animations_config_path_arg = DeclareLaunchArgument(
         "animations_config_path",
-        default_value=PathJoinSubstitution(
-            [FindPackageShare("panther_lights"), "config", "led_config.yaml"]
-        ),
+        default_value=PathJoinSubstitution([lights_pkg, "config", animations_config]),
         description="Path to a YAML file with a description of led configuration.",
     )
 
@@ -45,6 +50,15 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
+    robot_model_dict = {"LNX": "lynx", "PTH": "panther"}
+    robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
+    declare_robot_model_arg = DeclareLaunchArgument(
+        "robot_model",
+        default_value=robot_model_dict[robot_model_env],
+        description="Specify robot model",
+        choices=["lynx", "panther"],
+    )
+
     use_sim = LaunchConfiguration("use_sim")
     declare_use_sim_arg = DeclareLaunchArgument(
         "use_sim",
@@ -59,6 +73,8 @@ def generate_launch_description():
         description="Path to a YAML file with a description of the user defined animations.",
     )
 
+    driver_config = PythonExpression(["'", robot_model, "_driver.yaml'"])
+    driver_config_path = PathJoinSubstitution([lights_pkg, "config", driver_config])
     lights_container = ComposableNodeContainer(
         package="rclcpp_components",
         name="lights_container",
@@ -71,6 +87,7 @@ def generate_launch_description():
                 name="lights_driver",
                 namespace=namespace,
                 remappings=[("/diagnostics", "diagnostics")],
+                parameters=[driver_config_path],
                 extra_arguments=[
                     {"use_intra_process_comms": True},
                 ],
@@ -95,6 +112,7 @@ def generate_launch_description():
     )
 
     actions = [
+        declare_robot_model_arg,  # robot_model is used by animations_config_path
         declare_animations_config_path_arg,
         declare_namespace_arg,
         declare_use_sim_arg,
diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp
index e014695a..7535955c 100644
--- a/panther_lights/src/lights_driver_node.cpp
+++ b/panther_lights/src/lights_driver_node.cpp
@@ -54,10 +54,12 @@ LightsDriverNode::LightsDriverNode(const rclcpp::NodeOptions & options)
 
   this->declare_parameter<double>("global_brightness", 1.0);
   this->declare_parameter<double>("frame_timeout", 0.1);
-  this->declare_parameter<int>("num_led", 46);
+  this->declare_parameter<int>("channel_1_num_led", 46);
+  this->declare_parameter<int>("channel_2_num_led", 46);
 
   frame_timeout_ = this->get_parameter("frame_timeout").as_double();
-  num_led_ = this->get_parameter("num_led").as_int();
+  channel_1_num_led_ = this->get_parameter("channel_1_num_led").as_int();
+  channel_2_num_led_ = this->get_parameter("channel_2_num_led").as_int();
 
   const float global_brightness = this->get_parameter("global_brightness").as_double();
   channel_1_->SetGlobalBrightness(global_brightness);
@@ -133,8 +135,8 @@ void LightsDriverNode::InitializationTimerCB()
 
 void LightsDriverNode::ClearLEDs()
 {
-  channel_1_->SetPanel(std::vector<std::uint8_t>(num_led_ * 4, 0));
-  channel_2_->SetPanel(std::vector<std::uint8_t>(num_led_ * 4, 0));
+  channel_1_->SetPanel(std::vector<std::uint8_t>(channel_1_num_led_ * 4, 0));
+  channel_2_->SetPanel(std::vector<std::uint8_t>(channel_2_num_led_ * 4, 0));
 }
 
 void LightsDriverNode::ToggleLEDControl(const bool enable)
@@ -211,7 +213,9 @@ void LightsDriverNode::FrameCB(
     message = "Incorrect image encoding ('" + msg->encoding + "')";
   } else if (msg->height != 1) {
     message = "Incorrect image height " + std::to_string(msg->height);
-  } else if (msg->width != (std::uint32_t)num_led_) {
+  } else if (
+    msg->width !=
+    (std::uint32_t)(panel_name == "channel_1" ? channel_1_num_led_ : channel_2_num_led_)) {
     message = "Incorrect image width " + std::to_string(msg->width);
   }
 
diff --git a/panther_lights/test/files/animation.png b/panther_lights/test/files/animation.png
new file mode 100644
index 0000000000000000000000000000000000000000..5886cfe1022b5657090ecdd55075c2cab06b0fb1
GIT binary patch
literal 900
zcmeAS@N?(olHy`uVBq!ia0vp^F+jY7gAGXLS#_UbU|?*?baoE#baqxKD9TUE%t>Wn
z(3n^|(bnUzgUr$R;H9owvbP+MC_2AQaSS;UxH_PPYhhT%)q_ti>1SM;uw;h?*E&{q
zd$HAgdgpdo?ci#9z}~UCd-9_0DL?ivyP$Ne`ElKz-QRaqvvZu)y1Zszp~K}|<H=rK
zJO8YbsAz34XzEZ_GCn<H@)8r9<JWC#YW8}6(0N~6-{-hB!SH~((0QZ7TRP1<mYe4`
zf3_=}GBtJTk2w`*mT(K%7sVF->@8k-mg#)Ytea<S3a3QqeBE&A@S2>gBf&=uLoaK_
zpV+yk=-#6>p|95z`~N!>;20nI%<8P*k_{cI9s&y&^l`1)FmXYs`cbF0Z<FQzJ=_+r
zG~H<(mxQHQKr`FUx9tC$@869&dXm#za|+9^c$w$PA{=j+*0w#ak+=4H&GNyXIlSt6
zd^VHu{3YhQqTX<D<THz3kM+IDtXJN!P~F+=>9!}!HAT7){FbZ@mg~5~#GIX%Keu*{
z9Yf?Rty*Ro8K#dysq3rvSl_sPdw;ss-@goYQ;H^KuYTVTj25;eZ+91l|3DUlv3kvC
zAjMhW5n0T@z;^_M8K-LVNdpDhOFVsD*`G7<a~Wx`%zfnzjLAk%7srr_TW{}1avgFI
zaJ|@dZ03W1;#qUrb-%a=m$es9FQ}N}$ipZ2k$34<CkB}v4b0*n)TGtf#P+>toiF*M
zNgjybgRsPtruh;PJEZlv-;4b@!0=<y!R<UcciNXVRP#-5Wu87c>EQuZnH^mRYo{~G
z?9gh;2deMqX|UE@#lA0%;SO)OZNdH%<v}_z2O7C6W<OwNegIbfX#!9+(9VJfOnexO
zf9u`qgMo%NFo!GtXlDefxu^SMc4Mo))SsgVJQ>$jJ@9ACvna6CyxU|i!N3mWm$QM4
z-~PayO^h$bud9{2V)BD<ww(|5d2cLu@M+;o%NVhHYCl>VTX#QP#cpTpkvl_a&xvU>
r%HM7lmlMka`vB}1klU!jn0k@lcD264o%Ls~fg;b-)z4*}Q$iB}%Kv?@

literal 0
HcmV?d00001

diff --git a/panther_lights/test/integration/panther_lights.test.py b/panther_lights/test/integration/panther_lights.test.py
index bcc52087..b3cac0a1 100644
--- a/panther_lights/test/integration/panther_lights.test.py
+++ b/panther_lights/test/integration/panther_lights.test.py
@@ -36,8 +36,11 @@
 
 def generate_test_description():
 
+    # TODO: Should be possibility to launch integration test for specific robot
     animations_config_path = (
-        PathJoinSubstitution([FindPackageShare("panther_lights"), "config", "led_config.yaml"]),
+        PathJoinSubstitution(
+            [FindPackageShare("panther_lights"), "config", "panther_animations.yaml"]
+        ),
     )
 
     lights_controller_node = Node(
diff --git a/panther_lights/test/unit/animation/test_image_animation.cpp b/panther_lights/test/unit/animation/test_image_animation.cpp
index 9437bae1..2409a30c 100644
--- a/panther_lights/test/unit/animation/test_image_animation.cpp
+++ b/panther_lights/test/unit/animation/test_image_animation.cpp
@@ -94,21 +94,21 @@ TEST_F(TestImageAnimation, ParseImagePath)
   EXPECT_EQ(this->test_image_path, animation_->ParseImagePath(this->test_image_path));
 
   // test ROS package path
-  image_path = "$(find invalid_ros_package)/animations/strip01_red.png";
+  image_path = "$(find invalid_ros_package)/test/files/animation.png";
   EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error);
 
   // invalid substitution
-  image_path = "$(fin panther_lights)/animations/strip01_red.png";
+  image_path = "$(fin panther_lights)/test/files/animation.png";
   EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error);
-  image_path = "$(find panther_lights/animations/strip01_red.png";
+  image_path = "$(find panther_lights/test/files/animation.png";
   EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error);
 
   // following ones may not work if ROS package is not build or sourced
-  image_path = "$(find panther_lights)/animations/strip01_red.png";
+  image_path = "$(find panther_lights)/test/files/animation.png";
   EXPECT_NO_THROW(animation_->ParseImagePath(image_path));
 
   // multiple spaces after find syntax
-  image_path = "$(find    panther_lights)/animations/strip01_red.png";
+  image_path = "$(find    panther_lights)/test/files/animation.png";
   EXPECT_NO_THROW(animation_->ParseImagePath(image_path));
 }
 
diff --git a/panther_lights/test/unit/led_components/test_led_animation.cpp b/panther_lights/test/unit/led_components/test_led_animation.cpp
index 64dd9b02..51dbc58d 100644
--- a/panther_lights/test/unit/led_components/test_led_animation.cpp
+++ b/panther_lights/test/unit/led_components/test_led_animation.cpp
@@ -54,7 +54,7 @@ TestLEDAnimation::TestLEDAnimation()
   anim_desc.segments = {kTestSegmentName1, kTestSegmentName2};
   anim_desc.type = "panther_lights::ImageAnimation";
   anim_desc.animation =
-    YAML::Load("{image: $(find panther_lights)/animations/triangle01_red.png, duration: 2.0}");
+    YAML::Load("{image: $(find panther_lights)/test/files/animation.png, duration: 2.0}");
 
   panther_lights::LEDAnimationDescription led_anim_desc;
   led_anim_desc.id = 0;
diff --git a/panther_lights/test/unit/led_components/test_led_animations_queue.cpp b/panther_lights/test/unit/led_components/test_led_animations_queue.cpp
index bdd9386d..4eb8d8c7 100644
--- a/panther_lights/test/unit/led_components/test_led_animations_queue.cpp
+++ b/panther_lights/test/unit/led_components/test_led_animations_queue.cpp
@@ -61,7 +61,7 @@ panther_lights::LEDAnimation TestLEDAnimationsQueue::CreateLEDAnimation(
   anim_desc.segments = {"segment_1", "segment_2"};
   anim_desc.type = "panther_lights::ImageAnimation";
   anim_desc.animation =
-    YAML::Load("{image: $(find panther_lights)/animations/triangle01_red.png, duration: 2.0}");
+    YAML::Load("{image: $(find panther_lights)/test/files/animation.png, duration: 2.0}");
 
   panther_lights::LEDAnimationDescription led_anim_desc;
   led_anim_desc.id = 0;
diff --git a/panther_lights/test/unit/led_components/test_led_segment.cpp b/panther_lights/test/unit/led_components/test_led_segment.cpp
index 9e20ff0d..82fbec3e 100644
--- a/panther_lights/test/unit/led_components/test_led_segment.cpp
+++ b/panther_lights/test/unit/led_components/test_led_segment.cpp
@@ -189,7 +189,7 @@ TEST_F(TestLEDSegment, SetAnimation)
 {
   // test each known animtion type
   const auto image_anim_desc = YAML::Load(
-    "{image: $(find panther_lights)/animations/triangle01_red.png, "
+    "{image: $(find panther_lights)/test/files/animation.png, "
     "duration: 2}");
   const auto charging_anim_desc = YAML::Load("{duration: 2}");
 
@@ -203,7 +203,7 @@ TEST_F(TestLEDSegment, SetAnimation)
 TEST_F(TestLEDSegment, SetAnimationRepeating)
 {
   const auto anim_desc = YAML::Load(
-    "{image: $(find panther_lights)/animations/triangle01_red.png, "
+    "{image: $(find panther_lights)/test/files/animation.png, "
     "duration: 2}");
   ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, false));
 
@@ -224,7 +224,7 @@ TEST_F(TestLEDSegment, UpdateAnimationAnimationNotSet)
 TEST_F(TestLEDSegment, UpdateAnimation)
 {
   const auto anim_desc = YAML::Load(
-    "{image: $(find panther_lights)/animations/triangle01_red.png, "
+    "{image: $(find panther_lights)/test/files/animation.png, "
     "duration: 2}");
   ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, false));
   EXPECT_NO_THROW(led_segment_->UpdateAnimation());
@@ -240,7 +240,7 @@ int main(int argc, char ** argv)
 TEST_F(TestLEDSegment, ResetDefaultAnimationWhenNewArrive)
 {
   const auto anim_desc = YAML::Load(
-    "{image: $(find panther_lights)/animations/triangle01_red.png, "
+    "{image: $(find panther_lights)/test/files/animation.png, "
     "duration: 2}");
   ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, true));
 
diff --git a/panther_lights/test/unit/led_components/test_segment_converter.cpp b/panther_lights/test/unit/led_components/test_segment_converter.cpp
index ef1a7747..807f9440 100644
--- a/panther_lights/test/unit/led_components/test_segment_converter.cpp
+++ b/panther_lights/test/unit/led_components/test_segment_converter.cpp
@@ -63,9 +63,7 @@ YAML::Node TestSegmentConverter::CreateSegmentDescription(
 
 YAML::Node TestSegmentConverter::CreateImageAnimationDescription()
 {
-  return YAML::Load(
-    "{image: $(find panther_lights)/animations/triangle01_red.png, "
-    "duration: 2}");
+  return YAML::Load("{image: $(find panther_lights)/test/files/animation.png, duration: 2}");
 }
 
 TEST_F(TestSegmentConverter, ConvertInvalidChannel)
diff --git a/panther_lights/test/unit/test_lights_controller_node.cpp b/panther_lights/test/unit/test_lights_controller_node.cpp
index 37fcff3b..99b0a069 100644
--- a/panther_lights/test/unit/test_lights_controller_node.cpp
+++ b/panther_lights/test/unit/test_lights_controller_node.cpp
@@ -83,7 +83,7 @@ class TestLightsControllerNode : public testing::Test
 
 TestLightsControllerNode::TestLightsControllerNode()
 {
-  animations_config_path_ = testing::TempDir() + "/led_config.yaml";
+  animations_config_path_ = testing::TempDir() + "/animations.yaml";
 
   CreateLEDConfig(animations_config_path_);
 
@@ -116,7 +116,7 @@ void TestLightsControllerNode::CreateLEDConfig(const std::filesystem::path file_
   segments_map["test"] = std::vector<std::string>(1, kTestSegmentName);
 
   YAML::Node animation;
-  animation["image"] = "$(find panther_lights)/animations/triangle01_red.png";
+  animation["image"] = "$(find panther_lights)/test/files/strip01_red.png";
   animation["duration"] = 2;
 
   YAML::Node animation_desc;
@@ -136,14 +136,14 @@ void TestLightsControllerNode::CreateLEDConfig(const std::filesystem::path file_
   led_animations.push_back(led_animation_0);
   led_animations.push_back(led_animation_1);
 
-  YAML::Node led_config;
-  led_config["panels"] = std::vector<YAML::Node>(1, panel);
-  led_config["segments"] = std::vector<YAML::Node>(1, segment);
-  led_config["segments_map"] = segments_map;
-  led_config["led_animations"] = led_animations;
+  YAML::Node animations_config;
+  animations_config["panels"] = std::vector<YAML::Node>(1, panel);
+  animations_config["segments"] = std::vector<YAML::Node>(1, segment);
+  animations_config["segments_map"] = segments_map;
+  animations_config["led_animations"] = led_animations;
 
   YAML::Emitter out;
-  out << led_config;
+  out << animations_config;
 
   std::ofstream fout(file_path);
   if (fout.is_open()) {
diff --git a/panther_lights/test/unit/test_lights_driver_node.cpp b/panther_lights/test/unit/test_lights_driver_node.cpp
index 9a20b0f5..09918acf 100644
--- a/panther_lights/test/unit/test_lights_driver_node.cpp
+++ b/panther_lights/test/unit/test_lights_driver_node.cpp
@@ -55,7 +55,8 @@ panther_lights::LightsDriverNode::LightsDriverNode(
   channel_2_(channel_2),
   diagnostic_updater_(this)
 {
-  num_led_ = 46;
+  channel_1_num_led_ = 46;
+  channel_2_num_led_ = 46;
   frame_timeout_ = 0.1;
 };
 
@@ -83,7 +84,10 @@ class DriverNodeWrapper : public panther_lights::LightsDriverNode
     return LightsDriverNode::FrameCB(msg, panel, last_time, panel_name);
   }
 
-  int GetNumLed() const { return num_led_; }
+  int GetNumLed(const std::string & panel_name) const
+  {
+    return panel_name == "channel_1" ? channel_1_num_led_ : channel_2_num_led_;
+  }
   double GetTimeout() const { return frame_timeout_; }
   bool GetLedControlGranted() const { return led_control_granted_; }
   bool GetLedControlPending() const { return led_control_pending_; }
@@ -98,7 +102,7 @@ class TestLightsDriverNode : public testing::Test
   TestLightsDriverNode();
   ~TestLightsDriverNode() {};
 
-  ImageMsg::UniquePtr CreateValidImageMsg();
+  ImageMsg::UniquePtr CreateValidImageMsg(const std::string & panel_name);
   std::shared_future<std::pair<SetBoolSrv::Request::SharedPtr, SetBoolSrv::Response::SharedPtr>>
   CreateSetBoolSrvFuture(bool request_data, bool response_success);
 
@@ -115,7 +119,7 @@ TestLightsDriverNode::TestLightsDriverNode()
   lights_driver_node_ = std::make_unique<DriverNodeWrapper>(channel_1_, channel_2_);
 }
 
-ImageMsg::UniquePtr TestLightsDriverNode::CreateValidImageMsg()
+ImageMsg::UniquePtr TestLightsDriverNode::CreateValidImageMsg(const std::string & panel_name)
 {
   ImageMsg::UniquePtr msg(new ImageMsg);
 
@@ -123,7 +127,7 @@ ImageMsg::UniquePtr TestLightsDriverNode::CreateValidImageMsg()
   msg->header.stamp = lights_driver_node_->now();
   msg->header.frame_id = "image_frame";
   msg->height = 1;
-  msg->width = lights_driver_node_->GetNumLed();
+  msg->width = lights_driver_node_->GetNumLed(panel_name);
   msg->encoding = sensor_msgs::image_encodings::RGBA8;
   msg->is_bigendian = false;
   msg->step = msg->width * 4;
@@ -158,11 +162,13 @@ TEST_F(TestLightsDriverNode, TestInitialization)
 
 TEST_F(TestLightsDriverNode, ClearLEDs)
 {
-  auto num_led = lights_driver_node_->GetNumLed();
-  std::vector<std::uint8_t> zero_frame(num_led * 4, 0);
+  auto num_led_1 = lights_driver_node_->GetNumLed("channel_1");
+  auto num_led_2 = lights_driver_node_->GetNumLed("channel_2");
+  std::vector<std::uint8_t> zero_frame_1(num_led_1 * 4, 0);
+  std::vector<std::uint8_t> zero_frame_2(num_led_2 * 4, 0);
 
-  EXPECT_CALL(*channel_1_, SetPanel(zero_frame)).Times(1);
-  EXPECT_CALL(*channel_2_, SetPanel(zero_frame)).Times(1);
+  EXPECT_CALL(*channel_1_, SetPanel(zero_frame_1)).Times(1);
+  EXPECT_CALL(*channel_2_, SetPanel(zero_frame_2)).Times(1);
 
   lights_driver_node_->ClearLEDs();
 }
@@ -203,7 +209,7 @@ TEST_F(TestLightsDriverNode, ToggleLEDControlCBDisabled)
 
 TEST_F(TestLightsDriverNode, FrameCBSuccessNoControl)
 {
-  auto msg = CreateValidImageMsg();
+  auto msg = CreateValidImageMsg("channel_1");
 
   EXPECT_CALL(*channel_1_, SetPanel(testing::_)).Times(0);
 
@@ -212,7 +218,8 @@ TEST_F(TestLightsDriverNode, FrameCBSuccessNoControl)
 
 TEST_F(TestLightsDriverNode, FrameCBSuccess)
 {
-  auto msg = CreateValidImageMsg();
+  auto msg_1 = CreateValidImageMsg("channel_1");
+  auto msg_2 = CreateValidImageMsg("channel_2");
 
   auto future = CreateSetBoolSrvFuture(true, true);
   lights_driver_node_->ToggleLEDControlCB(std::move(future));
@@ -220,13 +227,13 @@ TEST_F(TestLightsDriverNode, FrameCBSuccess)
   EXPECT_CALL(*channel_1_, SetPanel(testing::_)).Times(1);
   EXPECT_CALL(*channel_2_, SetPanel(testing::_)).Times(1);
 
-  lights_driver_node_->FrameCB(msg, channel_1_, msg->header.stamp, "channel_1");
-  lights_driver_node_->FrameCB(msg, channel_2_, msg->header.stamp, "channel_2");
+  lights_driver_node_->FrameCB(msg_1, channel_1_, msg_1->header.stamp, "channel_1");
+  lights_driver_node_->FrameCB(msg_2, channel_2_, msg_2->header.stamp, "channel_2");
 }
 
 TEST_F(TestLightsDriverNode, FrameCBTimeout)
 {
-  auto msg = CreateValidImageMsg();
+  auto msg = CreateValidImageMsg("channel_1");
   auto timeout = lights_driver_node_->GetTimeout();
 
   // Set timestamp to exceed timeout
@@ -242,7 +249,7 @@ TEST_F(TestLightsDriverNode, FrameCBTimeout)
 
 TEST_F(TestLightsDriverNode, FrameCBPast)
 {
-  auto msg = CreateValidImageMsg();
+  auto msg = CreateValidImageMsg("channel_1");
 
   // Set last_time to be younger than msg timestamp
   auto future = CreateSetBoolSrvFuture(true, true);
@@ -255,7 +262,7 @@ TEST_F(TestLightsDriverNode, FrameCBPast)
 
 TEST_F(TestLightsDriverNode, FrameCBEncoding)
 {
-  auto msg = CreateValidImageMsg();
+  auto msg = CreateValidImageMsg("channel_1");
 
   // Set incorrect encoding
   msg->encoding = sensor_msgs::image_encodings::RGB8;
@@ -270,7 +277,7 @@ TEST_F(TestLightsDriverNode, FrameCBEncoding)
 
 TEST_F(TestLightsDriverNode, FrameCBHeight)
 {
-  auto msg = CreateValidImageMsg();
+  auto msg = CreateValidImageMsg("channel_1");
 
   // Set incorrect height
   msg->height = 2;
@@ -285,10 +292,10 @@ TEST_F(TestLightsDriverNode, FrameCBHeight)
 
 TEST_F(TestLightsDriverNode, FrameCBWidth)
 {
-  auto msg = CreateValidImageMsg();
+  auto msg = CreateValidImageMsg("channel_1");
 
   // Set incorrect width
-  msg->width = lights_driver_node_->GetNumLed() + 1;
+  msg->width = lights_driver_node_->GetNumLed("channel_1") + 1;
 
   auto future = CreateSetBoolSrvFuture(true, true);
   lights_driver_node_->ToggleLEDControlCB(std::move(future));

From 08c0a33ede8fb62f7ddb637a56f496bada667558 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 4 Oct 2024 11:06:42 +0200
Subject: [PATCH 068/100] Revert unnecessary changes

---
 README.md                                     |  3 +-
 .../launch/localization.launch.py             | 38 +++++++++++--------
 2 files changed, 24 insertions(+), 17 deletions(-)

diff --git a/README.md b/README.md
index 7c268f86..2ae60abb 100644
--- a/README.md
+++ b/README.md
@@ -92,11 +92,13 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | ✅   | ✅   | `components_config_path`     | Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options). <br/> ***string:*** [`components.yaml`](./panther_description/config/components.yaml) |
 | ✅   | ✅   | `controller_config_path`     | Path to controller configuration file. A path to custom configuration can be specified here. <br/> ***string:*** [`{wheel_type}_controller.yaml`](./panther_controller/config/)                                                                                                                                    |
 | ✅   | ✅   | `disable_manager`            | Enable or disable manager_bt_node. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                       |
+| ✅   | ✅   | `fuse_gps`                   | Include GPS for data fusion. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                             |
 | ❌   | ✅   | `gz_bridge_config_path`      | Path to the parameter_bridge configuration file. <br/> ***string:*** [`gz_bridge.yaml`](./panther_gazebo/config/gz_bridge.yaml)                                                                                                                                                                                    |
 | ❌   | ✅   | `gz_gui`                     | Run simulation with specific GUI layout. <br/> ***string:*** [`teleop.config`](https://github.com/husarion/husarion_gz_worlds/blob/main/config/teleop.config)                                                                                                                                                      |
 | ❌   | ✅   | `gz_headless_mode`           | Run the simulation in headless mode. Useful when a GUI is not needed or to reduce the number of calculations. <br/> ***bool:*** `False`                                                                                                                                                                            |
 | ❌   | ✅   | `gz_log_level`               | Adjust the level of console output. <br/> ***int:*** `1` (choices: `0`, `1`, `2`, `3`, `4`)                                                                                                                                                                                                                        |
 | ❌   | ✅   | `gz_world`                   | Absolute path to SDF world file. <br/> ***string:*** [`husarion_world.sdf`](https://github.com/husarion/husarion_gz_worlds/blob/main/worlds/husarion_world.sdf)                                                                                                                                                    |
+| ✅   | ✅   | `launch_nmea_gps`            | Whether to launch the NMEA NavSat driver node. Advisable when the robot is equipped with the [ANT02](https://husarion.com/manuals/panther/panther-options/#ant02---wi-fi--lte--gps). <br/> ***bool:*** `False`                                                                                                     |
 | ✅   | ✅   | `lights_bt_project_path`     | Path to BehaviorTree project file, responsible for lights management. <br/> ***string:*** [`PantherLightsBT.btproj`](./panther_manager/behavior_trees/PantherLightsBT.btproj)                                                                                                                                      |
 | ✅   | ✅   | `localization_config_path`   | Specify the path to the localization configuration file. <br/> ***string:*** [`relative_localization.yaml`](./panther_localization/config/relative_localization.yaml)                                                                                                                                              |
 | ✅   | ✅   | `localization_mode`          | Specifies the localization mode:  <br/>- 'relative' `odometry/filtered` data is relative to the initial position and orientation. <br/>- 'enu' `odometry/filtered` data is relative to initial position and ENU (East North Up) orientation. <br/> ***string:*** `relative` (choices: `relative`, `enu`)           |
@@ -106,7 +108,6 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | ✅   | ✅   | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`PantherSafetyBT.btproj`](./panther_manager/behavior_trees/PantherSafetyBT.btproj)                                                                                                                         |
 | ✅   | ✅   | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts_config.yaml`](./panther_manager/config/shutdown_hosts_config.yaml)                                                                                                                                                       |
 | ✅   | ✅   | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                    |
-| ✅   | ✅   | `use_gps`                    | Whether to launch the NMEA NavSat driver node. Advisable when the robot is equipped with the [ANT02](https://husarion.com/manuals/panther/panther-options/#ant02---wi-fi--lte--gps). <br/> ***bool:*** `False`                                                                                                     |
 | ✅   | ✅   | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                              |
 | ✅   | ✅   | `user_led_animations_path`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                    |
 | ✅   | ✅   | `wheel_config_path`          | Path to wheel configuration file. <br/> ***string:*** [`{wheel_type}.yaml`](./panther_description/config)                                                                                                                                                                                                          |
diff --git a/panther_localization/launch/localization.launch.py b/panther_localization/launch/localization.launch.py
index 5647c4e0..2645a6b4 100644
--- a/panther_localization/launch/localization.launch.py
+++ b/panther_localization/launch/localization.launch.py
@@ -29,6 +29,22 @@
 
 
 def generate_launch_description():
+    fuse_gps = LaunchConfiguration("fuse_gps")
+    declare_fuse_gps_arg = DeclareLaunchArgument(
+        "fuse_gps",
+        default_value="False",
+        description="Include GPS for data fusion",
+        choices=["True", "true", "False", "false"],
+    )
+
+    launch_nmea_gps = LaunchConfiguration("launch_nmea_gps")
+    declare_launch_nmea_gps_arg = DeclareLaunchArgument(
+        "launch_nmea_gps",
+        default_value="False",
+        description="Launch NMEA navsat gps driver",
+        choices=["True", "true", "False", "false"],
+    )
+
     localization_mode = LaunchConfiguration("localization_mode")
     declare_localization_mode_arg = DeclareLaunchArgument(
         "localization_mode",
@@ -48,17 +64,6 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
-    use_gps = LaunchConfiguration("use_gps")
-    declare_use_gps_arg = DeclareLaunchArgument(
-        "use_gps",
-        default_value="False",
-        description=(
-            "Launch NMEA node navsat gps driver and include GPS for data fusion. "
-            "Advisable when the robot is equipped with the [ANT02]"
-        ),
-        choices=["True", "true", "False", "false"],
-    )
-
     use_sim = LaunchConfiguration("use_sim")
     declare_use_sim_arg = DeclareLaunchArgument(
         "use_sim",
@@ -68,7 +73,7 @@ def generate_launch_description():
     )
 
     mode_prefix = PythonExpression(["'", localization_mode, "_'"])
-    gps_postfix = PythonExpression(["'_with_gps' if ", use_gps, " else ''"])
+    gps_postfix = PythonExpression(["'_with_gps' if ", fuse_gps, " else ''"])
     localization_config_filename = PythonExpression(
         ["'", mode_prefix, "localization", gps_postfix, ".yaml'"]
     )
@@ -103,7 +108,7 @@ def generate_launch_description():
             )
         ),
         launch_arguments={"namespace": namespace}.items(),
-        condition=IfCondition(use_gps),
+        condition=IfCondition(launch_nmea_gps),
     )
 
     navsat_transform_node = Node(
@@ -117,14 +122,15 @@ def generate_launch_description():
             ("gps/fix", "gps/fix"),
             ("odometry/gps", "_odometry/gps"),
         ],
-        condition=IfCondition(use_gps),
+        condition=IfCondition(fuse_gps),
     )
 
     actions = [
+        declare_fuse_gps_arg,
+        declare_launch_nmea_gps_arg,
         declare_localization_mode_arg,
-        declare_localization_config_path_arg,  # localization_config_path use use_gps and localization_mode
+        declare_localization_config_path_arg,  # localization_config_path use fuse_gps and localization_mode
         declare_namespace_arg,
-        declare_use_gps_arg,
         declare_use_sim_arg,
         SetParameter(name="use_sim_time", value=use_sim),
         ekf_filter_node,

From fc14d19de7d6489abdc8809f78f07083768a8b3e Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 4 Oct 2024 11:14:57 +0200
Subject: [PATCH 069/100] Move use_ekf to localization.launch.py

---
 panther_bringup/launch/bringup.launch.py           | 12 +-----------
 panther_gazebo/launch/simulate_robot.launch.py     | 10 ----------
 panther_localization/launch/localization.launch.py | 10 ++++++++++
 3 files changed, 11 insertions(+), 21 deletions(-)

diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py
index 37886eb7..d14d6f44 100644
--- a/panther_bringup/launch/bringup.launch.py
+++ b/panther_bringup/launch/bringup.launch.py
@@ -18,7 +18,7 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction
-from launch.conditions import IfCondition, UnlessCondition
+from launch.conditions import UnlessCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
     EnvironmentVariable,
@@ -45,14 +45,6 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
-    use_ekf = LaunchConfiguration("use_ekf")
-    declare_use_ekf_arg = DeclareLaunchArgument(
-        "use_ekf",
-        default_value="True",
-        description="Enable or disable EKF.",
-        choices=["True", "true", "False", "false"],
-    )
-
     robot_model = os.environ.get("ROBOT_MODEL", default="PTH")
     robot_model = "lynx" if robot_model == "LNX" else "panther"
     robot_serial_no = EnvironmentVariable(name="ROBOT_SERIAL_NO", default_value="----")
@@ -106,7 +98,6 @@ def generate_launch_description():
             )
         ),
         launch_arguments={"namespace": namespace}.items(),
-        condition=IfCondition(use_ekf),
     )
 
     manager_launch = IncludeLaunchDescription(
@@ -132,7 +123,6 @@ def generate_launch_description():
     actions = [
         declare_disable_manager_arg,
         declare_namespace_arg,
-        declare_use_ekf_arg,
         welcome_info,
         controller_launch,
         system_monitor_launch,
diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py
index edec27f2..fdde5156 100644
--- a/panther_gazebo/launch/simulate_robot.launch.py
+++ b/panther_gazebo/launch/simulate_robot.launch.py
@@ -103,14 +103,6 @@ def generate_launch_description():
         choices=["lynx", "panther"],
     )
 
-    use_ekf = LaunchConfiguration("use_ekf")
-    declare_use_ekf_arg = DeclareLaunchArgument(
-        "use_ekf",
-        default_value="True",
-        description="Enable or disable EKF.",
-        choices=["True", "true", "False", "false"],
-    )
-
     spawn_robot_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -170,7 +162,6 @@ def generate_launch_description():
             )
         ),
         launch_arguments={"namespace": namespace, "use_sim": "True"}.items(),
-        condition=IfCondition(use_ekf),
     )
 
     simulate_components = IncludeLaunchDescription(
@@ -243,7 +234,6 @@ def generate_launch_description():
         declare_disable_manager_arg,
         declare_gz_bridge_config_path_arg,
         declare_namespace_arg,
-        declare_use_ekf_arg,
         SetUseSimTime(True),
         spawn_robot_launch,
         lights_launch,
diff --git a/panther_localization/launch/localization.launch.py b/panther_localization/launch/localization.launch.py
index 2645a6b4..5447af64 100644
--- a/panther_localization/launch/localization.launch.py
+++ b/panther_localization/launch/localization.launch.py
@@ -64,6 +64,14 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
+    use_ekf = LaunchConfiguration("use_ekf")
+    declare_use_ekf_arg = DeclareLaunchArgument(
+        "use_ekf",
+        default_value="True",
+        description="Enable or disable EKF.",
+        choices=["True", "true", "False", "false"],
+    )
+
     use_sim = LaunchConfiguration("use_sim")
     declare_use_sim_arg = DeclareLaunchArgument(
         "use_sim",
@@ -99,6 +107,7 @@ def generate_launch_description():
             ("set_pose", "localization/set_pose"),
             ("toggle", "localization/toggle"),
         ],
+        condition=IfCondition(use_ekf),
     )
 
     nmea_navsat_launch = IncludeLaunchDescription(
@@ -131,6 +140,7 @@ def generate_launch_description():
         declare_localization_mode_arg,
         declare_localization_config_path_arg,  # localization_config_path use fuse_gps and localization_mode
         declare_namespace_arg,
+        declare_use_ekf_arg,
         declare_use_sim_arg,
         SetParameter(name="use_sim_time", value=use_sim),
         ekf_filter_node,

From f2b0458949a140a7bd7adced463df267831cc63b Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 4 Oct 2024 11:26:07 +0200
Subject: [PATCH 070/100] Push unsaved file

---
 README.md | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/README.md b/README.md
index 2ae60abb..0ec1b290 100644
--- a/README.md
+++ b/README.md
@@ -87,7 +87,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | --- | --- | ---------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
 | ❌   | ✅   | `add_wheel_joints`           | Flag enabling joint_state_publisher to publish information about the wheel position. Should be false when there is a controller that sends this information. <br/> ***bool:*** `False`                                                                                                                             |
 | ❌   | ✅   | `add_world_transform`        | Adds a world frame that connects the tf trees of individual robots (useful when running multiple robots). <br/> ***bool:*** `False`                                                                                                                                                                                |
-| ✅   | ✅   | `animations_config_path`     | Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. <br/> ***string:*** [`led_config.yaml`](./panther_lights/config/led_config.yaml)                                                                              |
+| ✅   | ✅   | `animations_config_path`     | Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. <br/> ***string:*** [`{robot_model}_animations.yaml`](./panther_lights/config)                                                                                |
 | ❌   | ✅   | `battery_config_path`        | Path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only. <br/> ***string:*** `None`                                                                                                                                                                |
 | ✅   | ✅   | `components_config_path`     | Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options). <br/> ***string:*** [`components.yaml`](./panther_description/config/components.yaml) |
 | ✅   | ✅   | `controller_config_path`     | Path to controller configuration file. A path to custom configuration can be specified here. <br/> ***string:*** [`{wheel_type}_controller.yaml`](./panther_controller/config/)                                                                                                                                    |
@@ -106,7 +106,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | ✅   | ✅   | `publish_robot_state`        | Whether to publish the default Panther robot description. <br/> ***bool:*** `True`                                                                                                                                                                                                                                 |
 | ❌   | ✅   | `robot_model`                | Specify robot model type. <br/> ***string:*** `env(ROBOT_MODEL)` if not specified `panther` (choices: `lynx`, `panther`)                                                                                                                                                                                           |
 | ✅   | ✅   | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`PantherSafetyBT.btproj`](./panther_manager/behavior_trees/PantherSafetyBT.btproj)                                                                                                                         |
-| ✅   | ✅   | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts_config.yaml`](./panther_manager/config/shutdown_hosts_config.yaml)                                                                                                                                                       |
+| ✅   | ✅   | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts.yaml`](./panther_manager/config/shutdown_hosts.yaml)                                                                                                                                                                     |
 | ✅   | ✅   | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                    |
 | ✅   | ✅   | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                              |
 | ✅   | ✅   | `user_led_animations_path`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                    |

From 5e66827edf107605ca72d49fd9d3d7d555388c51 Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Tue, 8 Oct 2024 10:19:30 +0000
Subject: [PATCH 071/100] fix wheel radius

---
 lynx_description/config/WH05.yaml              | 2 +-
 panther_controller/config/WH05_controller.yaml | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/lynx_description/config/WH05.yaml b/lynx_description/config/WH05.yaml
index fb49e17e..4e25988b 100644
--- a/lynx_description/config/WH05.yaml
+++ b/lynx_description/config/WH05.yaml
@@ -1,4 +1,4 @@
-wheel_radius: 0.1445
+wheel_radius: 0.1348
 wheel_width: 0.09
 wheel_separation: 0.45
 # TODO change inertia and mass when ready
diff --git a/panther_controller/config/WH05_controller.yaml b/panther_controller/config/WH05_controller.yaml
index f243bdd0..7499d0fb 100644
--- a/panther_controller/config/WH05_controller.yaml
+++ b/panther_controller/config/WH05_controller.yaml
@@ -30,7 +30,7 @@
       right_wheel_names: [fr_wheel_joint, rr_wheel_joint]
 
       wheel_separation: 0.45
-      wheel_radius: 0.1445
+      wheel_radius: 0.1348
 
       # For skid drive kinematics it will act as ICR coefficient, kinematic model with ICR
       # coefficient isn't totally accurate and this coefficient can differ for various ground types

From 996babae40310721c506bad44b04559d6a38f79f Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Wed, 9 Oct 2024 12:45:02 +0200
Subject: [PATCH 072/100] Add docs_new build check  to workflow (#419)

* Test action

* test v2

* test

* test

* test

* Fix action

* Add docs check action
---
 .github/workflows/release-candidate.yaml | 24 +++++---
 .github/workflows/release-project.yaml   | 75 +++++++-----------------
 2 files changed, 37 insertions(+), 62 deletions(-)

diff --git a/.github/workflows/release-candidate.yaml b/.github/workflows/release-candidate.yaml
index c1504ad1..f2744f57 100644
--- a/.github/workflows/release-candidate.yaml
+++ b/.github/workflows/release-candidate.yaml
@@ -5,17 +5,31 @@ on:
   workflow_dispatch:
     inputs:
       version:
-        description: Release candidate version. IMPORTANT - required format `X.X.X`, eg `2.0.1`.
+        description: Release candidate version (format `X.X.X`, e.g. `2.0.1`).
         required: true
       date:
-        description: Release candidate date stamp. IMPORTANT - required format `YYYYMMDD`, eg `20240430`.
+        description: Release candidate date stamp (format `YYYYMMDD`, e.g. `20240430`).
         required: true
 
 env:
   RC_BRANCH_NAME: ${{ github.event.inputs.version }}-${{ github.event.inputs.date }}
 
 jobs:
-    # TODO: Add unit testing for panther_ros when ready
+  check_docs:
+    name: Check docs build
+    runs-on: ubuntu-22.04
+    steps:
+      - name: Trigger repository build workflow
+        uses: convictional/trigger-workflow-and-wait@v1.6.1
+        with:
+          owner: husarion
+          repo: docs_new
+          github_token: ${{ secrets.GH_PAT }}
+          workflow_file_name: test-release.yml
+          ref: master
+          client_payload: '{"husarion_ugv_branch": "ros2-devel"}'
+
+  # TODO: Add unit testing for panther_ros when ready
   unit_test_panther_ros:
     name: Run unit tests for panther_ros
     runs-on: ubuntu-22.04
@@ -55,7 +69,6 @@ jobs:
           github_token: ${{ secrets.GH_PAT }}
           workflow_file_name: update-tags-in-compose.yaml
           ref: ${{ env.RC_BRANCH_NAME }}
-          wait_interval: 10
           client_payload: |
             {
               "branch_name": "${{ env.RC_BRANCH_NAME }}",
@@ -76,7 +89,6 @@ jobs:
           github_token: ${{ secrets.GH_PAT }}
           workflow_file_name: ros-docker-image.yaml
           ref: ${{ env.RC_BRANCH_NAME }}
-          wait_interval: 10
           client_payload: |
             {
               "panther_codebase_version": "${{ env.RC_BRANCH_NAME }}",
@@ -98,7 +110,6 @@ jobs:
           github_token: ${{ secrets.GH_PAT }}
           workflow_file_name: build_and_deploy_image.yaml
           ref: ${{ env.RC_BRANCH_NAME }}
-          wait_interval: 10
           client_payload: |
             {
               "dev_image": "true",
@@ -121,7 +132,6 @@ jobs:
           github_token: ${{ secrets.GH_PAT }}
           workflow_file_name: build_and_deploy_flash_os_image.yaml
           ref: ${{ env.RC_BRANCH_NAME }}
-          wait_interval: 10
           client_payload: |
             {
               "image_tag": "${{ github.event.inputs.version }}"
diff --git a/.github/workflows/release-project.yaml b/.github/workflows/release-project.yaml
index 47800b4f..25782ec7 100644
--- a/.github/workflows/release-project.yaml
+++ b/.github/workflows/release-project.yaml
@@ -30,11 +30,11 @@ env:
   MAIN_BRANCH: ros2
 
 jobs:
-  release_panther_msgs:
-    name: Release panther_msgs repository
+  release_project:
+    name: Release Husarion UGV project
     runs-on: ubuntu-22.04
     steps:
-      - name: Trigger repository release workflow
+      - name: Release panther_msgs repository
         uses: convictional/trigger-workflow-and-wait@v1.6.1
         with:
           owner: husarion
@@ -42,7 +42,6 @@ jobs:
           github_token: ${{ secrets.GH_PAT }}
           workflow_file_name: release-repository.yaml
           ref: ${{ env.RC_BRANCH_NAME }}
-          wait_interval: 10
           client_payload: |
             {
               "release_candidate": "${{ env.RC_BRANCH_NAME }}",
@@ -52,13 +51,7 @@ jobs:
               "prerelease": "${{ github.event.inputs.prerelease }}"
             }
 
-  release_panther_ros:
-    name: Release panther_ros repository
-    needs:
-      - release_panther_msgs
-    runs-on: ubuntu-22.04
-    steps:
-      - name: Trigger panther_ros release workflow
+      - name: Release panther_ros repository
         uses: convictional/trigger-workflow-and-wait@v1.6.1
         with:
           owner: husarion
@@ -66,7 +59,6 @@ jobs:
           github_token: ${{ secrets.GITHUB_TOKEN }} # Use the default GITHUB_TOKEN for local repository
           workflow_file_name: release-repository.yaml
           ref: ${{ env.RC_BRANCH_NAME }}
-          wait_interval: 10
           client_payload: |
             {
               "release_candidate": "${{ env.RC_BRANCH_NAME }}",
@@ -76,13 +68,7 @@ jobs:
               "prerelease": "${{ github.event.inputs.prerelease }}"
             }
 
-  rebuild_and_push_docker_images:
-    name: Rebuild panther docker images with new version
-    runs-on: ubuntu-22.04
-    needs:
-      - release_panther_ros
-    steps:
-      - name: Trigger repository build workflow
+      - name: Rebuild panther docker images with new version
         uses: convictional/trigger-workflow-and-wait@v1.6.1
         with:
           owner: husarion
@@ -90,7 +76,6 @@ jobs:
           github_token: ${{ secrets.GH_PAT }}
           workflow_file_name: ros-docker-image.yaml
           ref: ${{ env.RC_BRANCH_NAME }}
-          wait_interval: 10
           client_payload: |
             {
               "panther_codebase_version": "${{ github.event.inputs.version }}",
@@ -98,13 +83,7 @@ jobs:
               "target_distro": "humble"
             }
 
-  release_panther_docker:
-    name: Release panther-docker repository
-    needs:
-      - rebuild_and_push_docker_images
-    runs-on: ubuntu-22.04
-    steps:
-      - name: Trigger repository release workflow
+      - name: Release panther-docker repository
         uses: convictional/trigger-workflow-and-wait@v1.6.1
         with:
           owner: husarion
@@ -112,7 +91,6 @@ jobs:
           github_token: ${{ secrets.GH_PAT }}
           workflow_file_name: release-repository.yaml
           ref: ${{ env.RC_BRANCH_NAME }}
-          wait_interval: 10
           client_payload: |
             {
               "release_candidate": "${{ env.RC_BRANCH_NAME }}",
@@ -122,14 +100,7 @@ jobs:
               "prerelease": "${{ github.event.inputs.prerelease }}"
             }
 
-  release_panther_rpi_os_image:
-    name: Release panther-rpi-os-img repository
-    if: ${{ fromJSON(github.event.inputs.automatic_mode) == true }}
-    needs:
-      - release_panther_docker
-    runs-on: ubuntu-22.04
-    steps:
-      - name: Trigger repository release workflow
+      - name: Release panther-rpi-os-img repository
         uses: convictional/trigger-workflow-and-wait@v1.6.1
         with:
           owner: husarion
@@ -137,7 +108,6 @@ jobs:
           github_token: ${{ secrets.GH_PAT }}
           workflow_file_name: release-repository.yaml
           ref: ${{ env.RC_BRANCH_NAME }}
-          wait_interval: 10
           client_payload: |
             {
               "release_candidate": "${{ env.RC_BRANCH_NAME }}",
@@ -147,14 +117,8 @@ jobs:
               "prerelease": "${{ github.event.inputs.prerelease }}"
             }
 
-  build_and_publish_rpi_image:
-    name: Build panther system image
-    if: ${{ fromJSON(github.event.inputs.automatic_mode) == true }}
-    needs:
-      - release_panther_rpi_os_image
-    runs-on: ubuntu-22.04
-    steps:
-      - name: Trigger repository build workflow
+      - name: Build panther system image
+        if: ${{ fromJSON(github.event.inputs.automatic_mode) == true }}
         uses: convictional/trigger-workflow-and-wait@v1.6.1
         with:
           owner: husarion
@@ -162,7 +126,6 @@ jobs:
           github_token: ${{ secrets.GH_PAT }}
           workflow_file_name: build_and_deploy_image.yaml
           ref: ${{ env.MAIN_BRANCH }}
-          wait_interval: 10
           client_payload: |
             {
               "dev_image": "false",
@@ -170,14 +133,8 @@ jobs:
               "image_tag": "${{ github.event.inputs.version }}"
             }
 
-  build_and_publish_rpi_flash_os_image:
-    name: Build panther flash OS image
-    if: ${{ fromJSON(github.event.inputs.automatic_mode) == true }}
-    needs:
-      - build_and_publish_rpi_image
-    runs-on: ubuntu-22.04
-    steps:
-      - name: Trigger repository build workflow
+      - name: Build panther flash OS image
+        if: ${{ fromJSON(github.event.inputs.automatic_mode) == true }}
         uses: convictional/trigger-workflow-and-wait@v1.6.1
         with:
           owner: husarion
@@ -185,8 +142,16 @@ jobs:
           github_token: ${{ secrets.GH_PAT }}
           workflow_file_name: build_and_deploy_flash_os_image.yaml
           ref: ${{ env.MAIN_BRANCH }}
-          wait_interval: 10
           client_payload: |
             {
               "image_tag": "${{ github.event.inputs.version }}"
             }
+
+      - name: Trigger repository build workflow
+        uses: convictional/trigger-workflow-and-wait@v1.6.1
+        with:
+          owner: husarion
+          repo: docs_new
+          github_token: ${{ secrets.GH_PAT }}
+          workflow_file_name: build.yml
+          ref: master

From 07aa9af6db3ff67dd2826d3cff31fe7069dcb179 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Thu, 10 Oct 2024 10:01:10 +0200
Subject: [PATCH 073/100] Rename panther_can to user_can (#421)

---
 lynx_description/urdf/lynx_macro.urdf.xacro                  | 2 +-
 panther_description/urdf/panther_macro.urdf.xacro            | 2 +-
 panther_hardware_interfaces/README.md                        | 2 +-
 panther_hardware_interfaces/test/utils/mock_roboteq.hpp      | 2 +-
 panther_hardware_interfaces/test/utils/system_test_utils.hpp | 2 +-
 panther_hardware_interfaces/test/utils/test_constants.hpp    | 4 ++--
 6 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/lynx_description/urdf/lynx_macro.urdf.xacro b/lynx_description/urdf/lynx_macro.urdf.xacro
index 4e1fdf17..a057816d 100644
--- a/lynx_description/urdf/lynx_macro.urdf.xacro
+++ b/lynx_description/urdf/lynx_macro.urdf.xacro
@@ -75,7 +75,7 @@
           <!-- Max RPM speed set in the Roboteq driver (MXRPM parameter) -->
           <param name="max_rpm_motor_speed">3600.0</param>
 
-          <param name="can_interface_name">panther_can</param>
+          <param name="can_interface_name">robot_can</param>
           <param name="master_can_id">3</param>
           <param name="driver_can_id">1</param>
           <param name="sdo_operation_timeout_ms">100</param>
diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro
index de7f32ef..bef943de 100644
--- a/panther_description/urdf/panther_macro.urdf.xacro
+++ b/panther_description/urdf/panther_macro.urdf.xacro
@@ -74,7 +74,7 @@
           <!-- Max RPM speed set in the Roboteq driver (MXRPM parameter) -->
           <param name="max_rpm_motor_speed">3600.0</param>
 
-          <param name="can_interface_name">panther_can</param>
+          <param name="can_interface_name">robot_can</param>
           <param name="master_can_id">3</param>
           <param name="front_driver_can_id">1</param>
           <param name="rear_driver_can_id">2</param>
diff --git a/panther_hardware_interfaces/README.md b/panther_hardware_interfaces/README.md
index 545dd777..a7f49b4f 100644
--- a/panther_hardware_interfaces/README.md
+++ b/panther_hardware_interfaces/README.md
@@ -42,7 +42,7 @@ Physical properties
 
 CAN settings
 
-- `can_interface_name` [*string*, default: **panther_can**]: Name of the CAN interface.
+- `can_interface_name` [*string*, default: **robot_can**]: Name of the CAN interface.
 - `master_can_id` [*int*, default: **3**]: CAN ID of the master device (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).
 - `sdo_operation_timeout_ms` [*int*, default: **100**]: Timeout of the SDO operations, currently no SDO operation is required in RT operation, so this timeout can be set to a higher value.
 - `pdo_motor_states_timeout_ms` [*int*, default: **15**]: Depends on the frequency at which Roboteq is configured to send motor states (PDO 1 and 2) data. By default, there should be 10 **[ms]** between received data, if it takes more than `pdo_motor_states_timeout_ms`, a motor states read error is triggered. The default value is set to be expected period +50% margin.
diff --git a/panther_hardware_interfaces/test/utils/mock_roboteq.hpp b/panther_hardware_interfaces/test/utils/mock_roboteq.hpp
index c863daf4..f6607813 100644
--- a/panther_hardware_interfaces/test/utils/mock_roboteq.hpp
+++ b/panther_hardware_interfaces/test/utils/mock_roboteq.hpp
@@ -297,7 +297,7 @@ class MockRoboteq
         auto exec = loop.get_executor();
         lely::io::Timer timer(poll, exec, CLOCK_MONOTONIC);
 
-        lely::io::CanController ctrl("panther_can");
+        lely::io::CanController ctrl("robot_can");
 
         lely::io::CanChannel chan1(poll, exec);
         chan1.open(ctrl);
diff --git a/panther_hardware_interfaces/test/utils/system_test_utils.hpp b/panther_hardware_interfaces/test/utils/system_test_utils.hpp
index db294b4e..be79f931 100644
--- a/panther_hardware_interfaces/test/utils/system_test_utils.hpp
+++ b/panther_hardware_interfaces/test/utils/system_test_utils.hpp
@@ -145,7 +145,7 @@ hardware_interface::HardwareInfo GenerateDefaultHardwareInfo()
     {"max_rpm_motor_speed", "3600.0"},
 
     // CANopen settings
-    {"can_interface_name", "panther_can"},
+    {"can_interface_name", "robot_can"},
     {"master_can_id", "3"},
     {"pdo_motor_states_timeout_ms", "15"},
     {"pdo_driver_state_timeout_ms", "75"},
diff --git a/panther_hardware_interfaces/test/utils/test_constants.hpp b/panther_hardware_interfaces/test/utils/test_constants.hpp
index c3d4a0fa..5b65c55e 100644
--- a/panther_hardware_interfaces/test/utils/test_constants.hpp
+++ b/panther_hardware_interfaces/test/utils/test_constants.hpp
@@ -28,7 +28,7 @@ namespace panther_hardware_interfaces_test
 {
 
 const panther_hardware_interfaces::CANopenSettings kCANopenSettings{
-  "panther_can",
+  "robot_can",
   3,
   {{"default", 1}, {"front", 1}, {"rear", 2}},
   std::chrono::milliseconds(15),
@@ -72,7 +72,7 @@ const std::map<std::string, std::string> kDefaultParamMap = {
   {"gearbox_efficiency", "0.75"},
   {"motor_torque_constant", "0.11"},
   {"max_rpm_motor_speed", "3600.0"},
-  {"can_interface_name", "panther_can"},
+  {"can_interface_name", "robot_can"},
   {"master_can_id", "3"},
   {"front_driver_can_id", "1"},
   {"rear_driver_can_id", "2"},

From 85b72ffd14e682dbffc67a0d22794530bafe1939 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Thu, 10 Oct 2024 10:43:23 +0200
Subject: [PATCH 074/100] Ros2 husarion ugv v2 (#422)

* Fast light configuration

* Add lynx lights launch, fixed tests, unify arguments and config names

* Add animation and fix some typos

* Update README.md

* Horizontal flip fix

* Reaname all the pkgs and most of the files

* Fix simulation

* PantherImu -> PhidgetImu

* Error increase frequency

* Fix config path

* Fix order

* Rename folders: panther_system -> robot_system

* panther_battery -> husarion_ugv_battery

* panther_bringup -> husarion_ugv_bringup

* panther_controller -> husarion_ugv_controller

* panther_diagnostics -> husarion_ugv_diagnostics

* panther_gazebo -> husarion_ugv_gazebo & GzPantherSystem -> EStopSystem &
rename filenames

* panther_lights -> husarion_ugv_lights

* panther_localization -> husarion_ugv_localization

* panther_manager -> husarion_ugv_manager

* panther_utils -> husarion_ugv_utils

* panther_hardware_interfaces -> husarion_ugv_hardware_interfaces

* PantherImu -> PhidgetImu

* Merge branch 'ros2-lynx-devel' into ros2-husarion-ugv-v2

* Fix some text

* Fix diagram

* PantherSafetyBT -> SafetyBT, PantherLightsBT -> LightsBT

* Add suggestions

* add unsaved

* panther_can -> robot_can
---
 .docs/panther_ros2_api.drawio.svg             |   4 -
 .docs/ros2_system_design.drawio.svg           |   4 +
 .github/workflows/release-project.yaml        |   2 +-
 .github/workflows/run-unit-tests.yaml         |  12 +-
 .pre-commit-config.yaml                       |   2 +-
 README.md                                     |  38 ++--
 ROS_API.md                                    | 108 ++++-----
 {panther => husarion_ugv}/CHANGELOG.rst       |   0
 {panther => husarion_ugv}/CMakeLists.txt      |   2 +-
 husarion_ugv/README.md                        |   3 +
 .../hardware_deps.repos                       |   0
 {panther => husarion_ugv}/package.xml         |   8 +-
 .../simulation_deps.repos                     |   0
 .../CHANGELOG.rst                             |   0
 .../CMakeLists.txt                            |   6 +-
 .../README.md                                 |  18 +-
 .../husarion_ugv_battery}/adc_data_reader.hpp |  10 +-
 .../battery/adc_battery.hpp                   |  22 +-
 .../husarion_ugv_battery}/battery/battery.hpp |  10 +-
 .../battery/roboteq_battery.hpp               |  18 +-
 .../battery_driver_node.hpp                   |  16 +-
 .../battery_publisher/battery_publisher.hpp   |  10 +-
 .../dual_battery_publisher.hpp                |  14 +-
 .../single_battery_publisher.hpp              |  14 +-
 .../launch/battery.launch.py                  |   2 +-
 .../package.xml                               |   4 +-
 .../src/battery/adc_battery.cpp               |  16 +-
 .../src/battery/roboteq_battery.cpp           |  12 +-
 .../src/battery_driver_node.cpp               |  20 +-
 .../battery_publisher/battery_publisher.cpp   |   6 +-
 .../dual_battery_publisher.cpp                |  16 +-
 .../single_battery_publisher.cpp              |  10 +-
 .../src/main.cpp                              |   5 +-
 .../test/battery/test_adc_battery.cpp         |  20 +-
 .../test/battery/test_battery.cpp             |  11 +-
 .../test/battery/test_roboteq_battery.cpp     |  18 +-
 .../test_battery_publisher.cpp                |   6 +-
 .../test_dual_battery_publisher.cpp           |  32 +--
 .../test_single_battery_publisher.cpp         |  24 +-
 .../test/test_adc_data_reader.cpp             |   6 +-
 .../test_battery_driver_node_adc_dual.cpp     |  18 +-
 .../test_battery_driver_node_adc_single.cpp   |  16 +-
 .../test/test_battery_driver_node_roboteq.cpp |  12 +-
 .../test/utils/test_battery_driver_node.hpp   |  12 +-
 .../CHANGELOG.rst                             |   0
 .../CMakeLists.txt                            |   2 +-
 .../README.md                                 |   4 +-
 .../launch/bringup.launch.py                  |  14 +-
 .../package.xml                               |  18 +-
 .../CHANGELOG.rst                             |   0
 .../CMakeLists.txt                            |   2 +-
 husarion_ugv_controller/CONFIGURATION.md      |   9 +
 .../README.md                                 |   4 +-
 .../config/WH01_controller.yaml               |   0
 .../config/WH02_controller.yaml               |   0
 .../config/WH04_controller.yaml               |   2 +-
 .../config/WH05_controller.yaml               |   0
 .../launch/controller.launch.py               |   8 +-
 .../package.xml                               |   6 +-
 .../CHANGELOG.rst                             |   0
 .../CMakeLists.txt                            |   4 +-
 .../README.md                                 |   4 +-
 .../cmake/SuperBuild.cmake                    |   2 +-
 .../config/system_monitor.yaml                |   0
 .../husarion_ugv_diagnostics}/filesystem.hpp  |  10 +-
 .../system_monitor_node.hpp                   |  14 +-
 .../husarion_ugv_diagnostics}/types.hpp       |  10 +-
 .../launch/system_monitor.launch.py           |   2 +-
 .../package.xml                               |   6 +-
 .../src/main.cpp                              |   8 +-
 .../src/system_monitor_node.cpp               |  24 +-
 .../integration/system_monitor_node.test.py   |   2 +-
 .../test/unit/test_filesystem.cpp             |   7 +-
 .../test/unit/test_system_monitor_node.cpp    |  26 +--
 .../CHANGELOG.rst                             |   0
 .../CMakeLists.txt                            |  18 +-
 .../CONFIGURATION.md                          |  12 +-
 .../README.md                                 |   4 +-
 .../config/battery_plugin.yaml                |   4 +-
 .../config/gz_bridge.yaml                     |   0
 .../config/teleop_with_estop.config           |   0
 .../hooks/setup_envs.sh.in                    |   0
 .../husarion_ugv_gazebo/estop_system.hpp      |  14 +-
 .../husarion_ugv_gazebo}/gui/e_stop.hpp       |  10 +-
 .../husarion_ugv_gazebo}/led_strip.hpp        |  10 +-
 .../launch/simulate_robot.launch.py           |  20 +-
 .../launch/simulation.launch.py               |   4 +-
 .../launch/spawn_robot.launch.py              |   2 +-
 .../package.xml                               |  14 +-
 .../plugins.xml                               |   6 +-
 .../src/estop_system.cpp                      |  30 +--
 .../src/gui/EStop.qml                         |   0
 .../src/gui/EStop.qrc                         |   0
 .../src/gui/e_stop.cpp                        |   8 +-
 .../src/led_strip.cpp                         |  10 +-
 .../CHANGELOG.rst                             |   0
 .../CMakeLists.txt                            | 214 +++++++++---------
 .../CODE_STRUCTURE.md                         |  13 +-
 .../README.md                                 |  12 +-
 .../cmake/SuperBuild.cmake                    |   2 +-
 .../config/canopen_configuration.yaml         |   0
 .../config/master.dcf                         |   0
 .../config/phidgets_spatial_parameters.yaml   |   0
 .../roboteq_motor_controllers_v80_21a.eds     |   0
 .../husarion_ugv_hardware_interfaces.xml      |  25 ++
 .../phidget_imu_sensor/phidget_imu_sensor.hpp |  18 +-
 .../robot_system}/gpio/gpio_controller.hpp    |  18 +-
 .../robot_system}/gpio/gpio_driver.hpp        |  12 +-
 .../robot_system}/gpio/types.hpp              |  10 +-
 .../robot_system}/lynx_system.hpp             |  12 +-
 .../robot_system}/panther_system.hpp          |  12 +-
 .../robot_driver/canopen_manager.hpp          |  10 +-
 .../robot_system}/robot_driver/driver.hpp     |  10 +-
 .../robot_driver/lynx_robot_driver.hpp        |  16 +-
 .../robot_driver/panther_robot_driver.hpp     |  16 +-
 .../robot_driver/robot_driver.hpp             |  12 +-
 .../robot_driver/roboteq_data_converters.hpp  |  14 +-
 .../robot_driver/roboteq_driver.hpp           |  12 +-
 .../robot_driver/roboteq_error_filter.hpp     |  10 +-
 .../robot_driver/roboteq_robot_driver.hpp     |  18 +-
 .../robot_system}/system_e_stop.hpp           |  16 +-
 .../robot_system}/system_ros_interface.hpp    |  14 +-
 .../robot_system}/ugv_system.hpp              |  24 +-
 .../utils.hpp                                 |  10 +-
 .../package.xml                               |   6 +-
 .../phidget_imu_sensor/phidget_imu_sensor.cpp |  92 ++++----
 .../robot_system}/gpio/gpio_controller.cpp    |  10 +-
 .../src/robot_system}/gpio/gpio_driver.cpp    |  12 +-
 .../src/robot_system}/lynx_system.cpp         |  17 +-
 .../src/robot_system}/panther_system.cpp      |  18 +-
 .../robot_driver/canopen_manager.cpp          |  12 +-
 .../robot_driver/lynx_robot_driver.cpp        |  10 +-
 .../robot_driver/panther_robot_driver.cpp     |  10 +-
 .../robot_driver/roboteq_data_converters.cpp  |  14 +-
 .../robot_driver/roboteq_driver.cpp           |  10 +-
 .../robot_driver/roboteq_error_filter.cpp     |   6 +-
 .../robot_driver/roboteq_robot_driver.cpp     |  14 +-
 .../src/robot_system}/system_e_stop.cpp       |   6 +-
 .../robot_system}/system_ros_interface.cpp    |   8 +-
 .../src/robot_system}/ugv_system.cpp          |  16 +-
 .../src/utils.cpp                             |   6 +-
 .../test/config/canopen_configuration.yaml    |   0
 .../test/config/slave_1.bin                   | Bin
 .../test_phidget_imu_sensor.cpp               | 178 +++++++--------
 .../gpio/test_gpio_controller.cpp             |  12 +-
 .../robot_system}/gpio/test_gpio_driver.cpp   |  26 +--
 .../test/test_utils.cpp                       |  14 +-
 .../robot_driver/test_canopen_manager.cpp     |  20 +-
 .../robot_driver/test_lynx_robot_driver.cpp   |  57 ++---
 .../test_panther_robot_driver.cpp             |  78 +++----
 .../test_roboteq_data_converters.cpp          |  49 ++--
 .../robot_driver/test_roboteq_driver.cpp      |  68 +++---
 .../test_roboteq_error_filter.cpp             |  34 +--
 .../test_roboteq_robot_driver.cpp             | 108 ++++-----
 .../unit/robot_system}/test_lynx_system.cpp   | 112 ++++-----
 .../robot_system}/test_panther_system.cpp     | 159 +++++++------
 .../test_system_ros_interface.cpp             |  27 +--
 .../unit/robot_system}/test_ugv_system.cpp    |  31 +--
 .../test/utils/fake_can_socket.hpp            |  10 +-
 .../test/utils/mock_driver.hpp                |  26 +--
 .../test/utils/mock_roboteq.hpp               |  14 +-
 .../test/utils/system_test_utils.hpp          |  41 ++--
 .../test/utils/test_constants.hpp             |  20 +-
 .../.docs/AUTONOMOUS_ACTION.webp              | Bin
 .../.docs/BATTERY_STATE.webp                  | Bin
 .../.docs/CHARGING_BATTERY.webp               | Bin
 .../.docs/CRITICAL_BATTERY.webp               | Bin
 .../.docs/ERROR.webp                          | Bin
 .../.docs/E_STOP.webp                         | Bin
 .../.docs/GOAL_ACHIEVED.webp                  | Bin
 .../.docs/LOW_BATTERY.webp                    | Bin
 .../.docs/MANUAL_ACTION.webp                  | Bin
 .../.docs/READY.webp                          | Bin
 .../CHANGELOG.rst                             |   0
 .../CMakeLists.txt                            |  43 ++--
 .../CONFIGURATION.md                          |  22 +-
 .../LIGHTS_API.md                             |   4 +-
 .../README.md                                 |  12 +-
 .../animations/lynx/battery_critical.png      | Bin
 .../animations/lynx/battery_low.png           | Bin
 .../animations/lynx/strip01_purple.png        | Bin
 .../animations/lynx/strip01_red.png           | Bin
 .../animations/lynx/triangle01_blue.png       | Bin
 .../animations/lynx/triangle01_cyan.png       | Bin
 .../animations/lynx/triangle01_green.png      | Bin
 .../animations/lynx/triangle01_orange.png     | Bin
 .../animations/lynx/triangle01_purple.png     | Bin
 .../animations/lynx/triangle01_red.png        | Bin
 .../animations/lynx/triangle01_yellow.png     | Bin
 .../animations/panther/battery_critical.png   | Bin
 .../animations/panther/battery_low.png        | Bin
 .../animations/panther/strip01_purple.png     | Bin
 .../animations/panther/strip01_red.png        | Bin
 .../animations/panther/triangle01_blue.png    | Bin
 .../animations/panther/triangle01_cyan.png    | Bin
 .../animations/panther/triangle01_green.png   | Bin
 .../animations/panther/triangle01_orange.png  | Bin
 .../animations/panther/triangle01_purple.png  | Bin
 .../animations/panther/triangle01_red.png     | Bin
 .../animations/panther/triangle01_yellow.png  | Bin
 .../config/lynx_animations.yaml               |  34 +--
 .../config/lynx_driver.yaml                   |   0
 .../config/panther_animations.yaml            |  34 +--
 .../config/panther_driver.yaml                |   0
 .../animation/animation.hpp                   |  16 +-
 .../animation/charging_animation.hpp          |  12 +-
 .../animation/image_animation.hpp             |  12 +-
 .../include/husarion_ugv_lights}/apa102.hpp   |  10 +-
 .../led_components/led_animations_queue.hpp   |  12 +-
 .../led_components/led_panel.hpp              |  10 +-
 .../led_components/led_segment.hpp            |  18 +-
 .../led_components/segment_converter.hpp      |  14 +-
 .../lights_controller_node.hpp                |  18 +-
 .../lights_driver_node.hpp                    |  12 +-
 .../launch/lights.launch.py                   |  10 +-
 .../package.xml                               |   6 +-
 husarion_ugv_lights/plugins.xml               |   8 +
 .../src/animation/charging_animation.cpp      |   8 +-
 .../src/animation/image_animation.cpp         |  14 +-
 .../src/apa102.cpp                            |   6 +-
 .../led_components/led_animations_queue.cpp   |   6 +-
 .../src/led_components/led_panel.cpp          |   6 +-
 .../src/led_components/led_segment.cpp        |  22 +-
 .../src/led_components/segment_converter.cpp  |  10 +-
 .../src/lights_controller_node.cpp            |  51 +++--
 .../src/lights_driver_node.cpp                |  10 +-
 .../test/files/animation.png                  | Bin
 .../integration/husarion_ugv_lights.test.py   |   8 +-
 .../test/unit/animation/test_animation.cpp    |   4 +-
 .../animation/test_charging_animation.cpp     |   4 +-
 .../unit/animation/test_image_animation.cpp   |  12 +-
 .../led_components/test_led_animation.cpp     |  30 +--
 .../test_led_animations_queue.cpp             |  82 +++----
 .../unit/led_components/test_led_panel.cpp    |   8 +-
 .../unit/led_components/test_led_segment.cpp  | 106 +++++----
 .../led_components/test_segment_converter.cpp |  54 ++---
 .../test/unit/test_apa102.cpp                 |   6 +-
 .../test/unit/test_lights_controller_node.cpp |  29 +--
 .../test/unit/test_lights_driver_node.cpp     |  14 +-
 .../CHANGELOG.rst                             |   0
 .../CMakeLists.txt                            |   2 +-
 .../README.md                                 |   2 +-
 .../config/enu_localization.yaml              |   2 +-
 .../config/enu_localization_with_gps.yaml     |   2 +-
 .../config/nmea_navsat.yaml                   |   0
 .../config/relative_localization.yaml         |   2 +-
 .../relative_localization_with_gps.yaml       |   2 +-
 .../launch/localization.launch.py             |   4 +-
 .../launch/nmea_navsat.launch.py              |   2 +-
 .../package.xml                               |   4 +-
 .../CHANGELOG.rst                             |   0
 .../CMakeLists.txt                            |  23 +-
 .../CONFIGURATION.md                          |   6 +-
 .../README.md                                 |  12 +-
 .../behavior_trees/LightsBT.btproj            |   2 +-
 .../behavior_trees/SafetyBT.btproj            |   2 +-
 .../behavior_trees/lights.xml                 |   0
 .../behavior_trees/safety.xml                 |   0
 .../behavior_trees/shutdown.xml               |   0
 .../config/lights_manager.yaml                |   0
 .../config/safety_manager.yaml                |   0
 .../config/shutdown_hosts.yaml                |   0
 .../behavior_tree_manager.hpp                 |  10 +-
 .../behavior_tree_utils.hpp                   |  14 +-
 .../lights_manager_node.hpp                   |  16 +-
 .../action/call_set_bool_service_node.hpp     |  10 +-
 .../call_set_led_animation_service_node.hpp   |  10 +-
 .../action/call_trigger_service_node.hpp      |  10 +-
 .../action/shutdown_hosts_from_file_node.hpp  |  16 +-
 .../action/shutdown_single_host_node.hpp      |  14 +-
 .../plugins/action/signal_shutdown_node.hpp   |  10 +-
 .../decorator/tick_after_timeout_node.hpp     |  10 +-
 .../plugins/shutdown_host.hpp                 |  10 +-
 .../plugins/shutdown_hosts_node.hpp           |  14 +-
 .../safety_manager_node.hpp                   |  20 +-
 .../launch/manager.launch.py                  |  16 +-
 .../package.xml                               |   6 +-
 .../src/behavior_tree_manager.cpp             |   6 +-
 .../src/lights_manager_node.cpp               |  22 +-
 .../src/lights_manager_node_main.cpp          |   5 +-
 .../action/call_set_bool_service_node.cpp     |  10 +-
 .../call_set_led_animation_service_node.cpp   |  10 +-
 .../action/call_trigger_service_node.cpp      |  10 +-
 .../action/shutdown_hosts_from_file_node.cpp  |  24 +-
 .../action/shutdown_single_host_node.cpp      |  12 +-
 .../plugins/action/signal_shutdown_node.cpp   |   8 +-
 .../decorator/tick_after_timeout_node.cpp     |   8 +-
 .../src/safety_manager_node.cpp               |  26 +--
 .../src/safety_manager_node_main.cpp          |   5 +-
 .../test_call_set_bool_service_node.cpp       |  16 +-
 ...st_call_set_led_animation_service_node.cpp |  26 ++-
 .../action/test_call_trigger_service_node.cpp |  14 +-
 .../test_shutdown_hosts_from_file_node.cpp    |  21 +-
 .../action/test_shutdown_single_host_node.cpp |  16 +-
 .../action/test_signal_shutdown_node.cpp      |   8 +-
 .../test_tick_after_timeout_node.cpp          |   6 +-
 .../test/plugins/test_shutdown_host.cpp       |  36 +--
 .../test/plugins/test_shutdown_hosts_node.cpp |  50 ++--
 .../test/test_behavior_tree_manager.cpp       |  10 +-
 .../test/test_behavior_tree_utils.cpp         |  10 +-
 .../test/test_lights_behavior_tree.cpp        |   4 +-
 .../test/test_lights_manager_node.cpp         |  10 +-
 .../test/test_safety_behavior_tree.cpp        |   4 +-
 .../test/test_safety_manager_node.cpp         |  16 +-
 .../test/utils/behavior_tree_test_utils.hpp   |   6 +-
 .../test/utils/plugin_test_utils.hpp          |  26 +--
 .../CHANGELOG.rst                             |   0
 .../CMakeLists.txt                            |   2 +-
 .../README.md                                 |   4 +-
 .../husarion_ugv_utils}/__init__.py           |   0
 .../integration_test_utils.py                 |   0
 .../husarion_ugv_utils}/messages.py           |   2 +-
 .../husarion_ugv_utils}/common_utilities.hpp  |  10 +-
 .../husarion_ugv_utils}/configure_rt.hpp      |  10 +-
 .../husarion_ugv_utils}/diagnostics.hpp       |  10 +-
 .../husarion_ugv_utils}/moving_average.hpp    |  10 +-
 .../include/husarion_ugv_utils}/ros_utils.hpp |  10 +-
 .../test/ros_test_utils.hpp                   |  10 +-
 .../husarion_ugv_utils}/test/test_utils.hpp   |  10 +-
 .../husarion_ugv_utils}/yaml_utils.hpp        |  10 +-
 .../package.xml                               |   2 +-
 .../test/test_common_utilities.cpp            |  41 ++--
 .../test/test_diagnostics.cpp                 |  12 +-
 .../test/test_moving_average.cpp              |  20 +-
 .../test/test_ros_test_utils.cpp              |   8 +-
 .../test/test_ros_utils.cpp                   |  27 +--
 .../test/test_test_utils.cpp                  |  28 +--
 .../test/test_yaml_utils.cpp                  |  36 +--
 lynx_description/launch/load_urdf.launch.py   |   4 +-
 lynx_description/urdf/lynx.urdf.xacro         |   2 +-
 lynx_description/urdf/lynx_macro.urdf.xacro   |   6 +-
 panther/README.md                             |   3 -
 panther_controller/CONFIGURATION.md           |   9 -
 .../launch/load_urdf.launch.py                |   4 +-
 panther_description/urdf/gazebo.urdf.xacro    |   2 +-
 panther_description/urdf/panther.urdf.xacro   |   2 +-
 .../urdf/panther_macro.urdf.xacro             |   6 +-
 .../panther_hardware_interfaces.xml           |  25 --
 .../test/utils/panther_system_test_utils.hpp  | 164 --------------
 panther_lights/plugins.xml                    |   8 -
 340 files changed, 2353 insertions(+), 2441 deletions(-)
 delete mode 100644 .docs/panther_ros2_api.drawio.svg
 create mode 100644 .docs/ros2_system_design.drawio.svg
 rename {panther => husarion_ugv}/CHANGELOG.rst (100%)
 rename {panther => husarion_ugv}/CMakeLists.txt (80%)
 create mode 100644 husarion_ugv/README.md
 rename panther/panther_hardware.repos => husarion_ugv/hardware_deps.repos (100%)
 rename {panther => husarion_ugv}/package.xml (71%)
 rename panther/panther_simulation.repos => husarion_ugv/simulation_deps.repos (100%)
 rename {panther_battery => husarion_ugv_battery}/CHANGELOG.rst (100%)
 rename {panther_battery => husarion_ugv_battery}/CMakeLists.txt (97%)
 rename {panther_battery => husarion_ugv_battery}/README.md (75%)
 rename {panther_battery/include/panther_battery => husarion_ugv_battery/include/husarion_ugv_battery}/adc_data_reader.hpp (90%)
 rename {panther_battery/include/panther_battery => husarion_ugv_battery/include/husarion_ugv_battery}/battery/adc_battery.hpp (83%)
 rename {panther_battery/include/panther_battery => husarion_ugv_battery/include/husarion_ugv_battery}/battery/battery.hpp (95%)
 rename {panther_battery/include/panther_battery => husarion_ugv_battery/include/husarion_ugv_battery}/battery/roboteq_battery.hpp (81%)
 rename {panther_battery/include/panther_battery => husarion_ugv_battery/include/husarion_ugv_battery}/battery_driver_node.hpp (80%)
 rename {panther_battery/include/panther_battery => husarion_ugv_battery/include/husarion_ugv_battery}/battery_publisher/battery_publisher.hpp (88%)
 rename {panther_battery/include/panther_battery => husarion_ugv_battery/include/husarion_ugv_battery}/battery_publisher/dual_battery_publisher.hpp (84%)
 rename {panther_battery/include/panther_battery => husarion_ugv_battery/include/husarion_ugv_battery}/battery_publisher/single_battery_publisher.hpp (78%)
 rename {panther_battery => husarion_ugv_battery}/launch/battery.launch.py (97%)
 rename {panther_battery => husarion_ugv_battery}/package.xml (93%)
 rename {panther_battery => husarion_ugv_battery}/src/battery/adc_battery.cpp (93%)
 rename {panther_battery => husarion_ugv_battery}/src/battery/roboteq_battery.cpp (94%)
 rename {panther_battery => husarion_ugv_battery}/src/battery_driver_node.cpp (91%)
 rename {panther_battery => husarion_ugv_battery}/src/battery_publisher/battery_publisher.cpp (96%)
 rename {panther_battery => husarion_ugv_battery}/src/battery_publisher/dual_battery_publisher.cpp (96%)
 rename {panther_battery => husarion_ugv_battery}/src/battery_publisher/single_battery_publisher.cpp (92%)
 rename {panther_battery => husarion_ugv_battery}/src/main.cpp (86%)
 rename {panther_battery => husarion_ugv_battery}/test/battery/test_adc_battery.cpp (93%)
 rename {panther_battery => husarion_ugv_battery}/test/battery/test_battery.cpp (93%)
 rename {panther_battery => husarion_ugv_battery}/test/battery/test_roboteq_battery.cpp (92%)
 rename {panther_battery => husarion_ugv_battery}/test/battery_publisher/test_battery_publisher.cpp (94%)
 rename {panther_battery => husarion_ugv_battery}/test/battery_publisher/test_dual_battery_publisher.cpp (93%)
 rename {panther_battery => husarion_ugv_battery}/test/battery_publisher/test_single_battery_publisher.cpp (75%)
 rename {panther_battery => husarion_ugv_battery}/test/test_adc_data_reader.cpp (92%)
 rename {panther_battery => husarion_ugv_battery}/test/test_battery_driver_node_adc_dual.cpp (91%)
 rename {panther_battery => husarion_ugv_battery}/test/test_battery_driver_node_adc_single.cpp (91%)
 rename {panther_battery => husarion_ugv_battery}/test/test_battery_driver_node_roboteq.cpp (93%)
 rename {panther_battery => husarion_ugv_battery}/test/utils/test_battery_driver_node.hpp (93%)
 rename {panther_bringup => husarion_ugv_bringup}/CHANGELOG.rst (100%)
 rename {panther_bringup => husarion_ugv_bringup}/CMakeLists.txt (83%)
 rename {panther_bringup => husarion_ugv_bringup}/README.md (87%)
 rename {panther_bringup => husarion_ugv_bringup}/launch/bringup.launch.py (87%)
 rename {panther_bringup => husarion_ugv_bringup}/package.xml (81%)
 rename {panther_controller => husarion_ugv_controller}/CHANGELOG.rst (100%)
 rename {panther_localization => husarion_ugv_controller}/CMakeLists.txt (82%)
 create mode 100644 husarion_ugv_controller/CONFIGURATION.md
 rename {panther_controller => husarion_ugv_controller}/README.md (96%)
 rename {panther_controller => husarion_ugv_controller}/config/WH01_controller.yaml (100%)
 rename {panther_controller => husarion_ugv_controller}/config/WH02_controller.yaml (100%)
 rename {panther_controller => husarion_ugv_controller}/config/WH04_controller.yaml (98%)
 rename {panther_controller => husarion_ugv_controller}/config/WH05_controller.yaml (100%)
 rename {panther_controller => husarion_ugv_controller}/launch/controller.launch.py (97%)
 rename {panther_controller => husarion_ugv_controller}/package.xml (89%)
 rename {panther_diagnostics => husarion_ugv_diagnostics}/CHANGELOG.rst (100%)
 rename {panther_diagnostics => husarion_ugv_diagnostics}/CMakeLists.txt (97%)
 rename {panther_diagnostics => husarion_ugv_diagnostics}/README.md (95%)
 rename {panther_diagnostics => husarion_ugv_diagnostics}/cmake/SuperBuild.cmake (97%)
 rename {panther_diagnostics => husarion_ugv_diagnostics}/config/system_monitor.yaml (100%)
 rename {panther_diagnostics/include/panther_diagnostics => husarion_ugv_diagnostics/include/husarion_ugv_diagnostics}/filesystem.hpp (93%)
 rename {panther_diagnostics/include/panther_diagnostics => husarion_ugv_diagnostics/include/husarion_ugv_diagnostics}/system_monitor_node.hpp (87%)
 rename {panther_diagnostics/include/panther_diagnostics => husarion_ugv_diagnostics/include/husarion_ugv_diagnostics}/types.hpp (82%)
 rename {panther_diagnostics => husarion_ugv_diagnostics}/launch/system_monitor.launch.py (97%)
 rename {panther_diagnostics => husarion_ugv_diagnostics}/package.xml (87%)
 rename {panther_diagnostics => husarion_ugv_diagnostics}/src/main.cpp (80%)
 rename {panther_diagnostics => husarion_ugv_diagnostics}/src/system_monitor_node.cpp (89%)
 rename {panther_diagnostics => husarion_ugv_diagnostics}/test/integration/system_monitor_node.test.py (97%)
 rename {panther_diagnostics => husarion_ugv_diagnostics}/test/unit/test_filesystem.cpp (91%)
 rename {panther_diagnostics => husarion_ugv_diagnostics}/test/unit/test_system_monitor_node.cpp (83%)
 rename {panther_gazebo => husarion_ugv_gazebo}/CHANGELOG.rst (100%)
 rename {panther_gazebo => husarion_ugv_gazebo}/CMakeLists.txt (79%)
 rename {panther_gazebo => husarion_ugv_gazebo}/CONFIGURATION.md (71%)
 rename {panther_gazebo => husarion_ugv_gazebo}/README.md (98%)
 rename {panther_gazebo => husarion_ugv_gazebo}/config/battery_plugin.yaml (82%)
 rename {panther_gazebo => husarion_ugv_gazebo}/config/gz_bridge.yaml (100%)
 rename {panther_gazebo => husarion_ugv_gazebo}/config/teleop_with_estop.config (100%)
 rename panther_gazebo/hooks/panther_gazebo.sh.in => husarion_ugv_gazebo/hooks/setup_envs.sh.in (100%)
 rename panther_gazebo/include/panther_gazebo/gz_panther_system.hpp => husarion_ugv_gazebo/include/husarion_ugv_gazebo/estop_system.hpp (87%)
 rename {panther_gazebo/include/panther_gazebo => husarion_ugv_gazebo/include/husarion_ugv_gazebo}/gui/e_stop.hpp (89%)
 rename {panther_gazebo/include/panther_gazebo => husarion_ugv_gazebo/include/husarion_ugv_gazebo}/led_strip.hpp (96%)
 rename {panther_gazebo => husarion_ugv_gazebo}/launch/simulate_robot.launch.py (91%)
 rename {panther_gazebo => husarion_ugv_gazebo}/launch/simulation.launch.py (94%)
 rename {panther_gazebo => husarion_ugv_gazebo}/launch/spawn_robot.launch.py (98%)
 rename {panther_gazebo => husarion_ugv_gazebo}/package.xml (91%)
 rename panther_gazebo/panther_simulation_plugins.xml => husarion_ugv_gazebo/plugins.xml (62%)
 rename panther_gazebo/src/gz_panther_system.cpp => husarion_ugv_gazebo/src/estop_system.cpp (82%)
 rename {panther_gazebo => husarion_ugv_gazebo}/src/gui/EStop.qml (100%)
 rename {panther_gazebo => husarion_ugv_gazebo}/src/gui/EStop.qrc (100%)
 rename {panther_gazebo => husarion_ugv_gazebo}/src/gui/e_stop.cpp (94%)
 rename {panther_gazebo => husarion_ugv_gazebo}/src/led_strip.cpp (97%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/CHANGELOG.rst (100%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/CMakeLists.txt (58%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/CODE_STRUCTURE.md (88%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/README.md (95%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/cmake/SuperBuild.cmake (98%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/config/canopen_configuration.yaml (100%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/config/master.dcf (100%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/config/phidgets_spatial_parameters.yaml (100%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/config/roboteq_motor_controllers_v80_21a.eds (100%)
 create mode 100644 husarion_ugv_hardware_interfaces/husarion_ugv_hardware_interfaces.xml
 rename panther_hardware_interfaces/include/panther_hardware_interfaces/panther_imu_sensor/panther_imu_sensor.hpp => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/phidget_imu_sensor/phidget_imu_sensor.hpp (91%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/gpio/gpio_controller.hpp (95%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/gpio/gpio_driver.hpp (95%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/gpio/types.hpp (87%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/lynx_system.hpp (80%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/panther_system.hpp (80%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/robot_driver/canopen_manager.hpp (91%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/robot_driver/driver.hpp (91%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/robot_driver/lynx_robot_driver.hpp (76%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/robot_driver/panther_robot_driver.hpp (77%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/robot_driver/robot_driver.hpp (87%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/robot_driver/roboteq_data_converters.hpp (93%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/robot_driver/roboteq_driver.hpp (93%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/robot_driver/roboteq_error_filter.hpp (90%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/robot_driver/roboteq_robot_driver.hpp (87%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/system_e_stop.hpp (87%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/system_ros_interface.hpp (96%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system}/ugv_system.hpp (88%)
 rename {panther_hardware_interfaces/include/panther_hardware_interfaces => husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces}/utils.hpp (89%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/package.xml (92%)
 rename panther_hardware_interfaces/src/panther_imu_sensor/panther_imu_sensor.cpp => husarion_ugv_hardware_interfaces/src/phidget_imu_sensor/phidget_imu_sensor.cpp (86%)
 rename {panther_hardware_interfaces/src/panther_system => husarion_ugv_hardware_interfaces/src/robot_system}/gpio/gpio_controller.cpp (96%)
 rename {panther_hardware_interfaces/src/panther_system => husarion_ugv_hardware_interfaces/src/robot_system}/gpio/gpio_driver.cpp (95%)
 rename {panther_hardware_interfaces/src/panther_system => husarion_ugv_hardware_interfaces/src/robot_system}/lynx_system.cpp (92%)
 rename {panther_hardware_interfaces/src/panther_system => husarion_ugv_hardware_interfaces/src/robot_system}/panther_system.cpp (93%)
 rename {panther_hardware_interfaces/src/panther_system => husarion_ugv_hardware_interfaces/src/robot_system}/robot_driver/canopen_manager.cpp (92%)
 rename {panther_hardware_interfaces/src/panther_system => husarion_ugv_hardware_interfaces/src/robot_system}/robot_driver/lynx_robot_driver.cpp (86%)
 rename {panther_hardware_interfaces/src/panther_system => husarion_ugv_hardware_interfaces/src/robot_system}/robot_driver/panther_robot_driver.cpp (90%)
 rename {panther_hardware_interfaces/src/panther_system => husarion_ugv_hardware_interfaces/src/robot_system}/robot_driver/roboteq_data_converters.cpp (95%)
 rename {panther_hardware_interfaces/src/panther_system => husarion_ugv_hardware_interfaces/src/robot_system}/robot_driver/roboteq_driver.cpp (96%)
 rename {panther_hardware_interfaces/src/panther_system => husarion_ugv_hardware_interfaces/src/robot_system}/robot_driver/roboteq_error_filter.cpp (93%)
 rename {panther_hardware_interfaces/src/panther_system => husarion_ugv_hardware_interfaces/src/robot_system}/robot_driver/roboteq_robot_driver.cpp (93%)
 rename {panther_hardware_interfaces/src/panther_system => husarion_ugv_hardware_interfaces/src/robot_system}/system_e_stop.cpp (94%)
 rename {panther_hardware_interfaces/src/panther_system => husarion_ugv_hardware_interfaces/src/robot_system}/system_ros_interface.cpp (97%)
 rename {panther_hardware_interfaces/src/panther_system => husarion_ugv_hardware_interfaces/src/robot_system}/ugv_system.cpp (97%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/src/utils.cpp (93%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/test/config/canopen_configuration.yaml (100%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/test/config/slave_1.bin (100%)
 rename panther_hardware_interfaces/test/panther_imu_sensor/test_panther_imu_sensor.cpp => husarion_ugv_hardware_interfaces/test/phidget_imu_sensor/test_phidget_imu_sensor.cpp (73%)
 rename {panther_hardware_interfaces/test/panther_system => husarion_ugv_hardware_interfaces/test/robot_system}/gpio/test_gpio_controller.cpp (93%)
 rename {panther_hardware_interfaces/test/panther_system => husarion_ugv_hardware_interfaces/test/robot_system}/gpio/test_gpio_driver.cpp (84%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/test/test_utils.cpp (86%)
 rename {panther_hardware_interfaces/test/unit/panther_system => husarion_ugv_hardware_interfaces/test/unit/robot_system}/robot_driver/test_canopen_manager.cpp (74%)
 rename {panther_hardware_interfaces/test/unit/panther_system => husarion_ugv_hardware_interfaces/test/unit/robot_system}/robot_driver/test_lynx_robot_driver.cpp (58%)
 rename {panther_hardware_interfaces/test/unit/panther_system => husarion_ugv_hardware_interfaces/test/unit/robot_system}/robot_driver/test_panther_robot_driver.cpp (55%)
 rename {panther_hardware_interfaces/test/unit/panther_system => husarion_ugv_hardware_interfaces/test/unit/robot_system}/robot_driver/test_roboteq_data_converters.cpp (86%)
 rename {panther_hardware_interfaces/test/unit/panther_system => husarion_ugv_hardware_interfaces/test/unit/robot_system}/robot_driver/test_roboteq_driver.cpp (80%)
 rename {panther_hardware_interfaces/test/unit/panther_system => husarion_ugv_hardware_interfaces/test/unit/robot_system}/robot_driver/test_roboteq_error_filter.cpp (87%)
 rename {panther_hardware_interfaces/test/unit/panther_system => husarion_ugv_hardware_interfaces/test/unit/robot_system}/robot_driver/test_roboteq_robot_driver.cpp (81%)
 rename {panther_hardware_interfaces/test/unit/panther_system => husarion_ugv_hardware_interfaces/test/unit/robot_system}/test_lynx_system.cpp (64%)
 rename {panther_hardware_interfaces/test/unit/panther_system => husarion_ugv_hardware_interfaces/test/unit/robot_system}/test_panther_system.cpp (59%)
 rename {panther_hardware_interfaces/test/unit/panther_system => husarion_ugv_hardware_interfaces/test/unit/robot_system}/test_system_ros_interface.cpp (87%)
 rename {panther_hardware_interfaces/test/unit/panther_system => husarion_ugv_hardware_interfaces/test/unit/robot_system}/test_ugv_system.cpp (90%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/test/utils/fake_can_socket.hpp (86%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/test/utils/mock_driver.hpp (63%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/test/utils/mock_roboteq.hpp (95%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/test/utils/system_test_utils.hpp (75%)
 rename {panther_hardware_interfaces => husarion_ugv_hardware_interfaces}/test/utils/test_constants.hpp (78%)
 rename {panther_lights => husarion_ugv_lights}/.docs/AUTONOMOUS_ACTION.webp (100%)
 rename {panther_lights => husarion_ugv_lights}/.docs/BATTERY_STATE.webp (100%)
 rename {panther_lights => husarion_ugv_lights}/.docs/CHARGING_BATTERY.webp (100%)
 rename {panther_lights => husarion_ugv_lights}/.docs/CRITICAL_BATTERY.webp (100%)
 rename {panther_lights => husarion_ugv_lights}/.docs/ERROR.webp (100%)
 rename {panther_lights => husarion_ugv_lights}/.docs/E_STOP.webp (100%)
 rename {panther_lights => husarion_ugv_lights}/.docs/GOAL_ACHIEVED.webp (100%)
 rename {panther_lights => husarion_ugv_lights}/.docs/LOW_BATTERY.webp (100%)
 rename {panther_lights => husarion_ugv_lights}/.docs/MANUAL_ACTION.webp (100%)
 rename {panther_lights => husarion_ugv_lights}/.docs/READY.webp (100%)
 rename {panther_lights => husarion_ugv_lights}/CHANGELOG.rst (100%)
 rename {panther_lights => husarion_ugv_lights}/CMakeLists.txt (86%)
 rename {panther_lights => husarion_ugv_lights}/CONFIGURATION.md (88%)
 rename {panther_lights => husarion_ugv_lights}/LIGHTS_API.md (92%)
 rename {panther_lights => husarion_ugv_lights}/README.md (81%)
 rename {panther_lights => husarion_ugv_lights}/animations/lynx/battery_critical.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/lynx/battery_low.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/lynx/strip01_purple.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/lynx/strip01_red.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/lynx/triangle01_blue.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/lynx/triangle01_cyan.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/lynx/triangle01_green.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/lynx/triangle01_orange.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/lynx/triangle01_purple.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/lynx/triangle01_red.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/lynx/triangle01_yellow.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/panther/battery_critical.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/panther/battery_low.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/panther/strip01_purple.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/panther/strip01_red.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/panther/triangle01_blue.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/panther/triangle01_cyan.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/panther/triangle01_green.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/panther/triangle01_orange.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/panther/triangle01_purple.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/panther/triangle01_red.png (100%)
 rename {panther_lights => husarion_ugv_lights}/animations/panther/triangle01_yellow.png (100%)
 rename {panther_lights => husarion_ugv_lights}/config/lynx_animations.yaml (62%)
 rename {panther_lights => husarion_ugv_lights}/config/lynx_driver.yaml (100%)
 rename {panther_lights => husarion_ugv_lights}/config/panther_animations.yaml (60%)
 rename {panther_lights => husarion_ugv_lights}/config/panther_driver.yaml (100%)
 rename {panther_lights/include/panther_lights => husarion_ugv_lights/include/husarion_ugv_lights}/animation/animation.hpp (91%)
 rename {panther_lights/include/panther_lights => husarion_ugv_lights/include/husarion_ugv_lights}/animation/charging_animation.hpp (83%)
 rename {panther_lights/include/panther_lights => husarion_ugv_lights/include/husarion_ugv_lights}/animation/image_animation.hpp (88%)
 rename {panther_lights/include/panther_lights => husarion_ugv_lights/include/husarion_ugv_lights}/apa102.hpp (96%)
 rename {panther_lights/include/panther_lights => husarion_ugv_lights/include/husarion_ugv_lights}/led_components/led_animations_queue.hpp (94%)
 rename {panther_lights/include/panther_lights => husarion_ugv_lights/include/husarion_ugv_lights}/led_components/led_panel.hpp (85%)
 rename {panther_lights/include/panther_lights => husarion_ugv_lights/include/husarion_ugv_lights}/led_components/led_segment.hpp (87%)
 rename {panther_lights/include/panther_lights => husarion_ugv_lights/include/husarion_ugv_lights}/led_components/segment_converter.hpp (70%)
 rename {panther_lights/include/panther_lights => husarion_ugv_lights/include/husarion_ugv_lights}/lights_controller_node.hpp (91%)
 rename {panther_lights/include/panther_lights => husarion_ugv_lights/include/husarion_ugv_lights}/lights_driver_node.hpp (94%)
 rename {panther_lights => husarion_ugv_lights}/launch/lights.launch.py (93%)
 rename {panther_lights => husarion_ugv_lights}/package.xml (88%)
 create mode 100644 husarion_ugv_lights/plugins.xml
 rename {panther_lights => husarion_ugv_lights}/src/animation/charging_animation.cpp (95%)
 rename {panther_lights => husarion_ugv_lights}/src/animation/image_animation.cpp (93%)
 rename {panther_lights => husarion_ugv_lights}/src/apa102.cpp (97%)
 rename {panther_lights => husarion_ugv_lights}/src/led_components/led_animations_queue.cpp (96%)
 rename {panther_lights => husarion_ugv_lights}/src/led_components/led_panel.cpp (93%)
 rename {panther_lights => husarion_ugv_lights}/src/led_components/led_segment.cpp (86%)
 rename {panther_lights => husarion_ugv_lights}/src/led_components/segment_converter.cpp (85%)
 rename {panther_lights => husarion_ugv_lights}/src/lights_controller_node.cpp (86%)
 rename {panther_lights => husarion_ugv_lights}/src/lights_driver_node.cpp (97%)
 rename {panther_lights => husarion_ugv_lights}/test/files/animation.png (100%)
 rename panther_lights/test/integration/panther_lights.test.py => husarion_ugv_lights/test/integration/husarion_ugv_lights.test.py (95%)
 rename {panther_lights => husarion_ugv_lights}/test/unit/animation/test_animation.cpp (98%)
 rename {panther_lights => husarion_ugv_lights}/test/unit/animation/test_charging_animation.cpp (97%)
 rename {panther_lights => husarion_ugv_lights}/test/unit/animation/test_image_animation.cpp (94%)
 rename {panther_lights => husarion_ugv_lights}/test/unit/led_components/test_led_animation.cpp (79%)
 rename {panther_lights => husarion_ugv_lights}/test/unit/led_components/test_led_animations_queue.cpp (66%)
 rename {panther_lights => husarion_ugv_lights}/test/unit/led_components/test_led_panel.cpp (93%)
 rename {panther_lights => husarion_ugv_lights}/test/unit/led_components/test_led_segment.cpp (57%)
 rename {panther_lights => husarion_ugv_lights}/test/unit/led_components/test_segment_converter.cpp (69%)
 rename {panther_lights => husarion_ugv_lights}/test/unit/test_apa102.cpp (97%)
 rename {panther_lights => husarion_ugv_lights}/test/unit/test_lights_controller_node.cpp (87%)
 rename {panther_lights => husarion_ugv_lights}/test/unit/test_lights_driver_node.cpp (95%)
 rename {panther_localization => husarion_ugv_localization}/CHANGELOG.rst (100%)
 rename {panther_controller => husarion_ugv_localization}/CMakeLists.txt (82%)
 rename {panther_localization => husarion_ugv_localization}/README.md (98%)
 rename {panther_localization => husarion_ugv_localization}/config/enu_localization.yaml (99%)
 rename {panther_localization => husarion_ugv_localization}/config/enu_localization_with_gps.yaml (99%)
 rename {panther_localization => husarion_ugv_localization}/config/nmea_navsat.yaml (100%)
 rename {panther_localization => husarion_ugv_localization}/config/relative_localization.yaml (99%)
 rename {panther_localization => husarion_ugv_localization}/config/relative_localization_with_gps.yaml (99%)
 rename {panther_localization => husarion_ugv_localization}/launch/localization.launch.py (96%)
 rename {panther_localization => husarion_ugv_localization}/launch/nmea_navsat.launch.py (96%)
 rename {panther_localization => husarion_ugv_localization}/package.xml (88%)
 rename {panther_manager => husarion_ugv_manager}/CHANGELOG.rst (100%)
 rename {panther_manager => husarion_ugv_manager}/CMakeLists.txt (95%)
 rename {panther_manager => husarion_ugv_manager}/CONFIGURATION.md (96%)
 rename {panther_manager => husarion_ugv_manager}/README.md (92%)
 rename panther_manager/behavior_trees/PantherLightsBT.btproj => husarion_ugv_manager/behavior_trees/LightsBT.btproj (93%)
 rename panther_manager/behavior_trees/PantherSafetyBT.btproj => husarion_ugv_manager/behavior_trees/SafetyBT.btproj (97%)
 rename {panther_manager => husarion_ugv_manager}/behavior_trees/lights.xml (100%)
 rename {panther_manager => husarion_ugv_manager}/behavior_trees/safety.xml (100%)
 rename {panther_manager => husarion_ugv_manager}/behavior_trees/shutdown.xml (100%)
 rename {panther_manager => husarion_ugv_manager}/config/lights_manager.yaml (100%)
 rename {panther_manager => husarion_ugv_manager}/config/safety_manager.yaml (100%)
 rename {panther_manager => husarion_ugv_manager}/config/shutdown_hosts.yaml (100%)
 rename {panther_manager/include/panther_manager => husarion_ugv_manager/include/husarion_ugv_manager}/behavior_tree_manager.hpp (92%)
 rename {panther_manager/include/panther_manager => husarion_ugv_manager/include/husarion_ugv_manager}/behavior_tree_utils.hpp (89%)
 rename {panther_manager/include/panther_manager => husarion_ugv_manager/include/husarion_ugv_manager}/lights_manager_node.hpp (84%)
 rename {panther_manager/include/panther_manager => husarion_ugv_manager/include/husarion_ugv_manager}/plugins/action/call_set_bool_service_node.hpp (81%)
 rename {panther_manager/include/panther_manager => husarion_ugv_manager/include/husarion_ugv_manager}/plugins/action/call_set_led_animation_service_node.hpp (82%)
 rename {panther_manager/include/panther_manager => husarion_ugv_manager/include/husarion_ugv_manager}/plugins/action/call_trigger_service_node.hpp (81%)
 rename {panther_manager/include/panther_manager => husarion_ugv_manager/include/husarion_ugv_manager}/plugins/action/shutdown_hosts_from_file_node.hpp (71%)
 rename {panther_manager/include/panther_manager => husarion_ugv_manager/include/husarion_ugv_manager}/plugins/action/shutdown_single_host_node.hpp (78%)
 rename {panther_manager/include/panther_manager => husarion_ugv_manager/include/husarion_ugv_manager}/plugins/action/signal_shutdown_node.hpp (79%)
 rename {panther_manager/include/panther_manager => husarion_ugv_manager/include/husarion_ugv_manager}/plugins/decorator/tick_after_timeout_node.hpp (80%)
 rename {panther_manager/include/panther_manager => husarion_ugv_manager/include/husarion_ugv_manager}/plugins/shutdown_host.hpp (96%)
 rename {panther_manager/include/panther_manager => husarion_ugv_manager/include/husarion_ugv_manager}/plugins/shutdown_hosts_node.hpp (93%)
 rename {panther_manager/include/panther_manager => husarion_ugv_manager/include/husarion_ugv_manager}/safety_manager_node.hpp (84%)
 rename {panther_manager => husarion_ugv_manager}/launch/manager.launch.py (86%)
 rename {panther_manager => husarion_ugv_manager}/package.xml (92%)
 rename {panther_manager => husarion_ugv_manager}/src/behavior_tree_manager.cpp (94%)
 rename {panther_manager => husarion_ugv_manager}/src/lights_manager_node.cpp (93%)
 rename {panther_manager => husarion_ugv_manager}/src/lights_manager_node_main.cpp (86%)
 rename {panther_manager => husarion_ugv_manager}/src/plugins/action/call_set_bool_service_node.cpp (84%)
 rename {panther_manager => husarion_ugv_manager}/src/plugins/action/call_set_led_animation_service_node.cpp (86%)
 rename {panther_manager => husarion_ugv_manager}/src/plugins/action/call_trigger_service_node.cpp (83%)
 rename {panther_manager => husarion_ugv_manager}/src/plugins/action/shutdown_hosts_from_file_node.cpp (71%)
 rename {panther_manager => husarion_ugv_manager}/src/plugins/action/shutdown_single_host_node.cpp (86%)
 rename {panther_manager => husarion_ugv_manager}/src/plugins/action/signal_shutdown_node.cpp (83%)
 rename {panther_manager => husarion_ugv_manager}/src/plugins/decorator/tick_after_timeout_node.cpp (87%)
 rename {panther_manager => husarion_ugv_manager}/src/safety_manager_node.cpp (93%)
 rename {panther_manager => husarion_ugv_manager}/src/safety_manager_node_main.cpp (86%)
 rename {panther_manager => husarion_ugv_manager}/test/plugins/action/test_call_set_bool_service_node.cpp (85%)
 rename {panther_manager => husarion_ugv_manager}/test/plugins/action/test_call_set_led_animation_service_node.cpp (86%)
 rename {panther_manager => husarion_ugv_manager}/test/plugins/action/test_call_trigger_service_node.cpp (84%)
 rename {panther_manager => husarion_ugv_manager}/test/plugins/action/test_shutdown_hosts_from_file_node.cpp (77%)
 rename {panther_manager => husarion_ugv_manager}/test/plugins/action/test_shutdown_single_host_node.cpp (81%)
 rename {panther_manager => husarion_ugv_manager}/test/plugins/action/test_signal_shutdown_node.cpp (85%)
 rename {panther_manager => husarion_ugv_manager}/test/plugins/decorator/test_tick_after_timeout_node.cpp (89%)
 rename {panther_manager => husarion_ugv_manager}/test/plugins/test_shutdown_host.cpp (64%)
 rename {panther_manager => husarion_ugv_manager}/test/plugins/test_shutdown_hosts_node.cpp (63%)
 rename {panther_manager => husarion_ugv_manager}/test/test_behavior_tree_manager.cpp (91%)
 rename {panther_manager => husarion_ugv_manager}/test/test_behavior_tree_utils.cpp (90%)
 rename {panther_manager => husarion_ugv_manager}/test/test_lights_behavior_tree.cpp (98%)
 rename {panther_manager => husarion_ugv_manager}/test/test_lights_manager_node.cpp (94%)
 rename {panther_manager => husarion_ugv_manager}/test/test_safety_behavior_tree.cpp (98%)
 rename {panther_manager => husarion_ugv_manager}/test/test_safety_manager_node.cpp (94%)
 rename {panther_manager => husarion_ugv_manager}/test/utils/behavior_tree_test_utils.hpp (88%)
 rename {panther_manager => husarion_ugv_manager}/test/utils/plugin_test_utils.hpp (80%)
 rename {panther_utils => husarion_ugv_utils}/CHANGELOG.rst (100%)
 rename {panther_utils => husarion_ugv_utils}/CMakeLists.txt (99%)
 rename {panther_utils => husarion_ugv_utils}/README.md (52%)
 rename {panther_utils/panther_utils => husarion_ugv_utils/husarion_ugv_utils}/__init__.py (100%)
 rename {panther_utils/panther_utils => husarion_ugv_utils/husarion_ugv_utils}/integration_test_utils.py (100%)
 rename {panther_utils/panther_utils => husarion_ugv_utils/husarion_ugv_utils}/messages.py (97%)
 rename {panther_utils/include/panther_utils => husarion_ugv_utils/include/husarion_ugv_utils}/common_utilities.hpp (93%)
 rename {panther_utils/include/panther_utils => husarion_ugv_utils/include/husarion_ugv_utils}/configure_rt.hpp (87%)
 rename {panther_utils/include/panther_utils => husarion_ugv_utils/include/husarion_ugv_utils}/diagnostics.hpp (88%)
 rename {panther_utils/include/panther_utils => husarion_ugv_utils/include/husarion_ugv_utils}/moving_average.hpp (86%)
 rename {panther_utils/include/panther_utils => husarion_ugv_utils/include/husarion_ugv_utils}/ros_utils.hpp (94%)
 rename {panther_utils/include/panther_utils => husarion_ugv_utils/include/husarion_ugv_utils}/test/ros_test_utils.hpp (93%)
 rename {panther_utils/include/panther_utils => husarion_ugv_utils/include/husarion_ugv_utils}/test/test_utils.hpp (90%)
 rename {panther_utils/include/panther_utils => husarion_ugv_utils/include/husarion_ugv_utils}/yaml_utils.hpp (91%)
 rename {panther_utils => husarion_ugv_utils}/package.xml (97%)
 rename {panther_utils => husarion_ugv_utils}/test/test_common_utilities.cpp (67%)
 rename {panther_utils => husarion_ugv_utils}/test/test_diagnostics.cpp (84%)
 rename {panther_utils => husarion_ugv_utils}/test/test_moving_average.cpp (80%)
 rename {panther_utils => husarion_ugv_utils}/test/test_ros_test_utils.cpp (84%)
 rename {panther_utils => husarion_ugv_utils}/test/test_ros_utils.cpp (76%)
 rename {panther_utils => husarion_ugv_utils}/test/test_test_utils.cpp (66%)
 rename {panther_utils => husarion_ugv_utils}/test/test_yaml_utils.cpp (53%)
 delete mode 100644 panther/README.md
 delete mode 100644 panther_controller/CONFIGURATION.md
 delete mode 100644 panther_hardware_interfaces/panther_hardware_interfaces.xml
 delete mode 100644 panther_hardware_interfaces/test/utils/panther_system_test_utils.hpp
 delete mode 100644 panther_lights/plugins.xml

diff --git a/.docs/panther_ros2_api.drawio.svg b/.docs/panther_ros2_api.drawio.svg
deleted file mode 100644
index 19f5bf61..00000000
--- a/.docs/panther_ros2_api.drawio.svg
+++ /dev/null
@@ -1,4 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<!-- Do not edit this file with editors other than draw.io -->
-<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
-<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="1932px" height="1041px" viewBox="-0.5 -0.5 1932 1041" content="&lt;mxfile host=&quot;app.diagrams.net&quot; agent=&quot;Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/124.0.0.0 Safari/537.36&quot; scale=&quot;1&quot; border=&quot;0&quot; version=&quot;24.7.6&quot;&gt;&#10;  &lt;diagram id=&quot;5C2LWovSXbSQWriEvJtN&quot; name=&quot;Page-1&quot;&gt;&#10;    &lt;mxGraphModel dx=&quot;1350&quot; dy=&quot;1328&quot; grid=&quot;1&quot; gridSize=&quot;10&quot; guides=&quot;1&quot; tooltips=&quot;1&quot; connect=&quot;1&quot; arrows=&quot;1&quot; fold=&quot;1&quot; page=&quot;0&quot; pageScale=&quot;1&quot; pageWidth=&quot;850&quot; pageHeight=&quot;1100&quot; math=&quot;0&quot; shadow=&quot;0&quot;&gt;&#10;      &lt;root&gt;&#10;        &lt;mxCell id=&quot;0&quot; /&gt;&#10;        &lt;mxCell id=&quot;1&quot; parent=&quot;0&quot; /&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-23&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_localization&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;imageHeight=24;arcSize=6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;970&quot; y=&quot;165.15&quot; width=&quot;180&quot; height=&quot;100&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-179&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_controller&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=#FFFFFF;strokeWidth=6;strokeColor=#E6E6E6;imageHeight=24;arcSize=6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;840&quot; y=&quot;320&quot; width=&quot;600&quot; height=&quot;240&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-199&quot; value=&quot;&amp;lt;span style=&amp;quot;font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_lights&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=top;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2400&quot; y=&quot;380&quot; width=&quot;200&quot; height=&quot;450&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-202&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_hardware_interfaces&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=5;strokeColor=#E6E6E6;imageHeight=24;arcSize=6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;840&quot; y=&quot;590&quot; width=&quot;1150&quot; height=&quot;360.12&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-206&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_battery&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=top;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2027.36&quot; y=&quot;610&quot; width=&quot;256&quot; height=&quot;140&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-208&quot; value=&quot;&amp;lt;span style=&amp;quot;font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_manager&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1978.87&quot; y=&quot;270&quot; width=&quot;243&quot; height=&quot;277.55&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-211&quot; value=&quot;&amp;lt;span style=&amp;quot;font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;panther_diagnostics&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=top;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2083.36&quot; y=&quot;110&quot; width=&quot;200&quot; height=&quot;120&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-24&quot; value=&quot;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot; style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;PantherSystem&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;align=center;labelBackgroundColor=none;fillColor=#CCCCCC;strokeColor=#E6E6E6;arcSize=6;verticalAlign=top;fontColor=#FFFFFF;gradientColor=none;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1155&quot; y=&quot;610.72&quot; width=&quot;815&quot; height=&quot;311.5&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; value=&quot;&amp;lt;span style=&amp;quot;color: rgb(255, 255, 255); text-align: left;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;hardware_controller&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ED3B42;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1660&quot; y=&quot;720.72&quot; width=&quot;285&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;GPIOController&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1416.88&quot; y=&quot;840.72&quot; width=&quot;160&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-28&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;MotorsController&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1345&quot; y=&quot;670.72&quot; width=&quot;190&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-29&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;CANopenController&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1176&quot; y=&quot;840.72&quot; width=&quot;200&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-30&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;RoboteqDriver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1201&quot; y=&quot;755.72&quot; width=&quot;150&quot; height=&quot;47.5&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-31&quot; value=&quot;CAN&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1207.25&quot; y=&quot;970.72&quot; width=&quot;137.5&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-32&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;labelBackgroundColor=none;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=16;fontColor=#888a85;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;fillColor=#FFFFFF;startArrow=block;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-29&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-31&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1204.25&quot; y=&quot;1210.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1291&quot; y=&quot;940.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-35&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=1;exitDx=0;exitDy=0;fillColor=#FFFFFF;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startArrow=block;endFill=1;&quot; parent=&quot;1&quot; source=&quot;NPlMt0osOJUE4kA7_BK4-15&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-79&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1501.13&quot; y=&quot;871.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1496.88&quot; y=&quot;970.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-38&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;EStopManager&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1415&quot; y=&quot;753.22&quot; width=&quot;160&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-56&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.488;entryY=-0.004;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.789;exitY=1;exitDx=0;exitDy=0;exitPerimeter=0;entryPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1496.78&quot; y=&quot;720.9200000000001&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1496.78&quot; y=&quot;753.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-58&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.789;exitY=1;exitDx=0;exitDy=0;exitPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1496.74&quot; y=&quot;803.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1496.74&quot; y=&quot;835.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-63&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.5;exitDx=0;exitDy=0;startFill=1;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-28&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-30&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1255&quot; y=&quot;688.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1255&quot; y=&quot;720.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-64&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;startFill=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-30&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-29&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1290&quot; y=&quot;810.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1291.34&quot; y=&quot;840.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-66&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.25;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;edgeStyle=orthogonalEdgeStyle;exitX=0;exitY=0.5;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-38&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1660&quot; y=&quot;766&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1593&quot; y=&quot;875.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-73&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.5;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.25;exitDx=0;exitDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-28&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1626&quot; y=&quot;695.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1545&quot; y=&quot;696.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-74&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.25;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.75;exitDx=0;exitDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1680&quot; y=&quot;797.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1599&quot; y=&quot;798.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;battery_driver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2057.36&quot; y=&quot;644.84&quot; width=&quot;190&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-77&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#888a85;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=0;exitDx=0;exitDy=0;fillColor=#FFFFFF;entryX=0.5;entryY=1;entryDx=0;entryDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-80&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2509.86&quot; y=&quot;1146.53&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2133.9400000000005&quot; y=&quot;720.7200000000001&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-79&quot; value=&quot;RPi GPIO&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1650&quot; y=&quot;970.72&quot; width=&quot;130&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-80&quot; value=&quot;ADC&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2083.61&quot; y=&quot;970.7199999999999&quot; width=&quot;137.5&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;imu_broadcaster&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;895&quot; y=&quot;474.72&quot; width=&quot;180&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;joint_state_broadcaster&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1190&quot; y=&quot;337.00000000000006&quot; width=&quot;240&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;drive_controller&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1070&quot; y=&quot;414.72&quot; width=&quot;180&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-95&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: left;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;controller_manager&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#FFFFFF;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;direction=north;horizontal=0;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;815&quot; y=&quot;330&quot; width=&quot;55&quot; height=&quot;220&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;daj83a5Ns2Ukew12XzJU-1&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;&quot; edge=&quot;1&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot;&gt;&#10;          &lt;mxGeometry relative=&quot;1&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;daj83a5Ns2Ukew12XzJU-2&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;&quot; edge=&quot;1&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot;&gt;&#10;          &lt;mxGeometry relative=&quot;1&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; value=&quot;&amp;lt;div style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot; style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;PantherImuSensor&amp;lt;/font&amp;gt;&amp;lt;/div&amp;gt;&amp;lt;div style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;align=center;labelBackgroundColor=none;fillColor=#CCCCCC;strokeColor=#E6E6E6;fontColor=#FFFFFF;arcSize=6;verticalAlign=top;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;860&quot; y=&quot;610.72&quot; width=&quot;250&quot; height=&quot;311.5&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-103&quot; value=&quot;IMU&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;916.25&quot; y=&quot;970.72&quot; width=&quot;137.5&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-106&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.5;exitY=1;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;660&quot; y=&quot;1060.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;721.25&quot; y=&quot;1060.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-116&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.25;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.687;exitY=0.972;exitDx=0;exitDy=0;exitPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-24&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1365&quot; y=&quot;380&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1365&quot; y=&quot;610.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-117&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.094;entryY=0.008;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.889;exitY=1.025;exitDx=0;exitDy=0;entryPerimeter=0;exitPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-24&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1235&quot; y=&quot;480.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1235&quot; y=&quot;610.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;ekf_filter&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;995&quot; y=&quot;175.72&quot; width=&quot;130&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-120&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;ImuFilter&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;880&quot; y=&quot;695.22&quot; width=&quot;210&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-121&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;Spatial&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;890&quot; y=&quot;821.34&quot; width=&quot;190&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-123&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=0;exitDx=0;exitDy=0;entryX=0.5;entryY=1;entryDx=0;entryDy=0;fillColor=default;endFill=1;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-121&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;984.5&quot; y=&quot;970.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;984.5&quot; y=&quot;922.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-124&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=0;exitDx=0;exitDy=0;entryX=0.5;entryY=1;entryDx=0;entryDy=0;fillColor=default;endFill=1;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-120&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;984.6600000000001&quot; y=&quot;821.34&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;984.6600000000001&quot; y=&quot;773.34&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-125&quot; value=&quot;&amp;lt;b&amp;gt;imu/data&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.25;exitY=0;exitDx=0;exitDy=0;align=right;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.8832&quot; y=&quot;15&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;940&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;975.47&quot; y=&quot;511.44000000000005&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-129&quot; value=&quot;&amp;lt;b&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;odometry/wheels&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;entryX=0.75;entryY=1;entryDx=0;entryDy=0;align=left;exitX=0.124;exitY=0.006;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.9376&quot; y=&quot;15&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1085&quot; y=&quot;305.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1085&quot; y=&quot;410.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-130&quot; value=&quot;&amp;lt;b&amp;gt;odometry/filtered&amp;lt;/b&amp;gt;&quot; style=&quot;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.25;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6758&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1028&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1040.3&quot; y=&quot;190.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-131&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.25;exitY=0;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.0769&quot; y=&quot;18&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;965&quot; y=&quot;40.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;965.47&quot; y=&quot;510.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-134&quot; value=&quot;&amp;lt;b&amp;gt;joint_states&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.588;exitY=0.013;exitDx=0;exitDy=0;align=right;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7994&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1330&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1330.0100000000002&quot; y=&quot;309.09000000000015&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-135&quot; value=&quot;&amp;lt;b&amp;gt;cmd_vel&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0.558;entryY=0.088;entryDx=0;entryDy=0;entryPerimeter=0;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.8578&quot; y=&quot;-15&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1170&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1170&quot; y=&quot;400&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-136&quot; value=&quot;&amp;lt;b&amp;gt;dynamic_joint_states&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.384;exitY=0.002;exitDx=0;exitDy=0;align=right;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.8069&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1281&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1280.0100000000002&quot; y=&quot;309.09000000000015&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-138&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;&amp;amp;nbsp;hardware/e_stop&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.876;exitY=-0.002;exitDx=0;exitDy=0;align=right;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; y=&quot;735.0912500000003&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-140&quot; value=&quot;&amp;lt;b&amp;gt;hardware/robot_driver_state&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.63;exitY=0.019;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1840&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1830&quot; y=&quot;710&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-143&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;&amp;amp;nbsp;hardware/io_state&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.75;exitY=0;exitDx=0;exitDy=0;align=right;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1874&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1870&quot; y=&quot;735.09&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-144&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;hardware/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;e_stop_trigger&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.071;exitY=0.001;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1680&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1680.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-145&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;hardware/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;e_stop_reset&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.213;exitY=-0.004;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1720&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1720.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-148&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;hardware/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;&amp;lt;i&amp;gt;&amp;amp;lt;hw_component&amp;amp;gt;&amp;lt;/i&amp;gt;_enable&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.353;exitY=0.029;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1760&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1760.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-149&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;robot_state_publisher&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1365&quot; y=&quot;200&quot; width=&quot;240&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-150&quot; value=&quot;&amp;lt;b&amp;gt;robot_description&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.673;exitY=-0.008;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6903&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1530&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1530.01&quot; y=&quot;199.99999999999997&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-151&quot; value=&quot;&amp;lt;b&amp;gt;/tf&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6915&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1440&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1440&quot; y=&quot;200&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-152&quot; value=&quot;&amp;lt;b&amp;gt;/tf_static&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.5;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-149&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.68&quot; y=&quot;12&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1485&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1482.21&quot; y=&quot;220.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-153&quot; value=&quot;&amp;lt;b&amp;gt;/tf&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.5;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6747&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1060&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1070&quot; y=&quot;190.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-156&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;battery/battery_status&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=1;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9179&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2310&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2230&quot; y=&quot;670&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-158&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;jumpStyle=none;jumpSize=6;exitX=0.75;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2275&quot; y=&quot;530.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; y=&quot;730&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1874&quot; y=&quot;670&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-159&quot; value=&quot;&amp;lt;div style=&amp;quot;font-size: 20px;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;&amp;quot;&amp;gt;Bumper Lights&amp;lt;/span&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=16;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2420&quot; y=&quot;970.7200000000001&quot; width=&quot;160&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights_driver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2410&quot; y=&quot;721.47&quot; width=&quot;180&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights_controller&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2410&quot; y=&quot;420&quot; width=&quot;180&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-162&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#888a85;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#FFFFFF;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startArrow=none;startFill=0;exitX=0.5;exitY=1;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-159&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2243.75&quot; y=&quot;915.8200000000002&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2243.41&quot; y=&quot;966.3200000000002&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-165&quot; value=&quot;&amp;lt;b style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;lights/channel_1_frame&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=16;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=left;entryX=0.5;entryY=0;entryDx=0;entryDy=0;exitX=0.5;exitY=1;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7234&quot; y=&quot;-20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2560&quot; y=&quot;853.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2320&quot; y=&quot;853.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-167&quot; value=&quot;&amp;lt;b style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;lights/channel_2_frame&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=16;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=left;exitX=0.75;exitY=1;exitDx=0;exitDy=0;entryX=0.75;entryY=0;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7234&quot; y=&quot;-20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2560&quot; y=&quot;883.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2606.98&quot; y=&quot;655.8200000000002&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-168&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: center; font-size: 18px;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial; font-size: 18px;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights/&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial; text-align: right; font-size: 18px;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;set_animation&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=1;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=0;exitY=0.75;exitDx=0;exitDy=0;entryX=1;entryY=0.75;entryDx=0;entryDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; target=&quot;D_R0GpuGBRVuPwS73e86-2&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.8077&quot; y=&quot;12&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2204.63&quot; y=&quot;469.2799999999999&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2208.14&quot; y=&quot;837.225&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-170&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: center;&amp;quot;&amp;gt;&amp;lt;b&amp;gt;lights/set_brightness&amp;lt;/b&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=1;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9279&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2620&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2430.0050000000006&quot; y=&quot;815&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;safety_manager&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2012&quot; y=&quot;304.72&quot; width=&quot;190&quot; height=&quot;100&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;system_monitor&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2093.35&quot; y=&quot;140.57&quot; width=&quot;180&quot; height=&quot;70&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-183&quot; value=&quot;&amp;lt;b&amp;gt;system_status&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0;exitY=0.5;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7278&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2040&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1940&quot; y=&quot;120.00000000000004&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2040&quot; y=&quot;130&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2040&quot; y=&quot;40&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-184&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0.145;entryY=0.01;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0;exitY=0.5;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9108&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2040&quot; y=&quot;280.7221600000001&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2080&quot; y=&quot;130&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2040&quot; y=&quot;176&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-187&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.876;exitY=-0.012;exitDx=0;exitDy=0;align=right;exitPerimeter=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2000&quot; y=&quot;420&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; y=&quot;735.0912500000003&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1910&quot; y=&quot;355&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-189&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0;entryY=0.25;entryDx=0;entryDy=0;exitX=0.75;exitY=0;exitDx=0;exitDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2009&quot; y=&quot;399&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1870&quot; y=&quot;730&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-190&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0;entryY=0;entryDx=0;entryDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2000&quot; y=&quot;380&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1840&quot; y=&quot;720&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1840&quot; y=&quot;305&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-191&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.353;exitY=0.023;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;entryX=0;entryY=1;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9453&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2009&quot; y=&quot;359&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1760.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1761&quot; y=&quot;405&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-195&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=1;align=left;exitX=1;exitY=0.25;exitDx=0;exitDy=0;entryX=1;entryY=0.5;entryDx=0;entryDy=0;fontStyle=1&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.0992&quot; y=&quot;-22&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2229.24&quot; y=&quot;400&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2149.24&quot; y=&quot;658&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2266&quot; y=&quot;668&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;668&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;331&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-204&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.59;exitY=0.007;exitDx=0;exitDy=0;align=right;exitPerimeter=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;jumpSize=6;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-149&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.8002&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1330&quot; y=&quot;10&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1330.0100000000002&quot; y=&quot;309.09000000000015&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1332&quot; y=&quot;300&quot; /&gt;&#10;              &lt;mxPoint x=&quot;1332&quot; y=&quot;230&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-214&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#676965;startArrow=none;startFill=0;endArrow=none;horizontal=1;align=left;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;verticalAlign=middle;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;780.0300000000002&quot; y=&quot;220.26000000000002&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;10&quot; y=&quot;10&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;720&quot; y=&quot;219.68999999999997&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;780&quot; y=&quot;219.91&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-215&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#676965;startArrow=none;startFill=0;endArrow=none;horizontal=1;align=left;endFill=0;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1&quot; y=&quot;-10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;780&quot; y=&quot;179.75&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;10&quot; y=&quot;-10&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;720&quot; y=&quot;179.84000000000003&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;D_R0GpuGBRVuPwS73e86-2&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights_manager&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ED3B42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2012&quot; y=&quot;420&quot; width=&quot;190&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;D_R0GpuGBRVuPwS73e86-7&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=1;align=left;entryX=1;entryY=0.25;entryDx=0;entryDy=0;fontStyle=1;exitX=1;exitY=0.25;exitDx=0;exitDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; target=&quot;D_R0GpuGBRVuPwS73e86-2&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.0992&quot; y=&quot;-22&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2340&quot; y=&quot;389.24&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2300&quot; y=&quot;700&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;668&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;443&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;D_R0GpuGBRVuPwS73e86-9&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0;entryY=0.5;entryDx=0;entryDy=0;exitX=0.876;exitY=0.004;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;D_R0GpuGBRVuPwS73e86-2&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;3091&quot; y=&quot;289.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2910&quot; y=&quot;695.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-3&quot; value=&quot;&amp;lt;span style=&amp;quot;forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;forced-color-adjust: none;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;forced-color-adjust: none; font-size: 16px;&amp;quot;&amp;gt;hardware/&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;span style=&amp;quot;forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; font-size: 16px;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;forced-color-adjust: none;&amp;quot;&amp;gt;led_control_enable&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=1;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;verticalAlign=middle;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9141&quot; y=&quot;-14&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2214.63&quot; y=&quot;1267.0600000000002&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2210&quot; y=&quot;898.3400000000001&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-6&quot; value=&quot;&amp;lt;b&amp;gt;/tf&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.389;exitY=0.011;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.857&quot; y=&quot;17&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1140&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1144.23&quot; y=&quot;190.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-8&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;battery/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;charging_status&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=1;exitY=0.5;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9189&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2340&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2306&quot; y=&quot;678&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-14&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=0.071;exitY=0.012;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0;entryY=0.75;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1690&quot; y=&quot;10&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1620&quot; y=&quot;520&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1680&quot; y=&quot;380&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-15&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;GPIODriver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1650&quot; y=&quot;840.72&quot; width=&quot;130&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-17&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.75;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.75;exitDx=0;exitDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;NPlMt0osOJUE4kA7_BK4-15&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1670&quot; y=&quot;797&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1587&quot; y=&quot;863&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-2&quot; value=&quot;&amp;lt;b&amp;gt;diagnostics&amp;lt;/b&amp;gt;&quot; style=&quot;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.75;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6747&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1093&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1070&quot; y=&quot;186&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-3&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: left;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial;&amp;quot;&amp;gt;&amp;lt;b&amp;gt;diagnostics&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=1;exitY=0.75;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9189&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2370&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2257&quot; y=&quot;700&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-4&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;diagnostics&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6688&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2070&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2090&quot; y=&quot;186&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-6&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: left;&amp;quot;&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9189&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2370&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2257&quot; y=&quot;722&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-7&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;diagnostics&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.5;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1802&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1840&quot; y=&quot;731&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-8&quot; value=&quot;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ED3B42;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;720&quot; y=&quot;45&quot; width=&quot;60&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-11&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 20px; font-weight: 700; text-wrap: wrap;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot;&amp;gt;Legend:&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;text;html=1;align=center;verticalAlign=middle;resizable=0;points=[];autosize=1;strokeColor=none;fillColor=none;fontSize=16;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;700&quot; y=&quot;-4.85&quot; width=&quot;100&quot; height=&quot;40&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-12&quot; value=&quot;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=#E6E6E6;fillColor=#CCCCCC;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;720&quot; y=&quot;85&quot; width=&quot;60&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-14&quot; value=&quot;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=#B0B3AC;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;720&quot; y=&quot;125&quot; width=&quot;60&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-18&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Nodes&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;45&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-19&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Plugin&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;85&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-20&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Objects&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;125&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-21&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Topics&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;165&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-22&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Services&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;205&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;Y5d-tTOP6FVwuIk47fc7-3&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=none;startWidth=5.555555555555555;endWidth=5.555555555555555;startFill=0;endFill=1;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;720&quot; y=&quot;260&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;780&quot; y=&quot;259.57&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;Y5d-tTOP6FVwuIk47fc7-4&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Data Transfer&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;245&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;      &lt;/root&gt;&#10;    &lt;/mxGraphModel&gt;&#10;  &lt;/diagram&gt;&#10;&lt;/mxfile&gt;&#10;" resource="https://app.diagrams.net/#G1LlhIvfnfv5NcG4CBD7yvBf7X81yUhw3e"><defs/><g><g data-cell-id="0"><g data-cell-id="1"><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-23"><g><rect x="270" y="175.15" width="180" height="100" rx="6" ry="6" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 272px; margin-left: 271px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">panther_localization</font></span></div></div></div></foreignObject><text x="360" y="272" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_localization</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-179"><g><rect x="140" y="330" width="600" height="240" rx="14.4" ry="14.4" fill-opacity="0.9" fill="#ffffff" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 598px; height: 1px; padding-top: 567px; margin-left: 141px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">panther_controller</font></span></div></div></div></foreignObject><text x="440" y="567" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-199"><g><rect x="1700" y="390" width="200" height="450" rx="30" ry="30" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 397px; margin-left: 1701px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;"><font color="#1a1a1a">panther_lights</font></span></div></div></div></foreignObject><text x="1800" y="409" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_lights</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-202"><g><rect x="140" y="600" width="1150" height="360.12" rx="21.61" ry="21.61" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="5" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 1148px; height: 1px; padding-top: 957px; margin-left: 141px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">panther_hardware_interfaces</font></span></div></div></div></foreignObject><text x="715" y="957" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_hardware_interfaces</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-206"><g><rect x="1327.36" y="620" width="256" height="140" rx="21" ry="21" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 254px; height: 1px; padding-top: 627px; margin-left: 1328px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">panther_battery</font></span></div></div></div></foreignObject><text x="1455" y="639" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_battery</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-208"><g><rect x="1278.87" y="280" width="243" height="277.55" rx="36.45" ry="36.45" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 241px; height: 1px; padding-top: 555px; margin-left: 1280px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;"><font color="#1a1a1a">panther_manager</font></span></div></div></div></foreignObject><text x="1400" y="555" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_manager</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-211"><g><rect x="1383.36" y="120" width="200" height="120" rx="18" ry="18" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 127px; margin-left: 1384px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;"><font color="#1a1a1a">panther_diagnostics</font></span></div></div></div></foreignObject><text x="1483" y="139" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">panther_diagnostics</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-24"><g><rect x="455" y="620.72" width="815" height="311.5" rx="18.69" ry="18.69" fill="#cccccc" stroke="#e6e6e6" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 813px; height: 1px; padding-top: 632px; margin-left: 456px;"><div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 24px;" color="#1a1a1a">PantherSystem</font></div></div></div></foreignObject><text x="863" y="651" fill="#FFFFFF" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">PantherSystem</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-26"><g><rect x="960" y="730.72" width="285" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 283px; height: 1px; padding-top: 778px; margin-left: 961px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><span style="color: rgb(255, 255, 255); text-align: left;"><font style="font-size: 18px;">hardware_controller</font></span></div></div></div></foreignObject><text x="1103" y="783" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">hardware_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-27"><g><rect x="716.88" y="850.72" width="160" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 878px; margin-left: 718px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">GPIOController</font></div></div></div></foreignObject><text x="797" y="883" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">GPIOController</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-28"><g><rect x="645" y="680.72" width="190" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 708px; margin-left: 646px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">MotorsController</font></div></div></div></foreignObject><text x="740" y="713" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">MotorsController</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-29"><g><rect x="476" y="850.72" width="200" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 878px; margin-left: 477px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">CANopenController</font></div></div></div></foreignObject><text x="576" y="883" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">CANopenController</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-30"><g><rect x="501" y="765.72" width="150" height="47.5" rx="7.13" ry="7.13" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 148px; height: 1px; padding-top: 791px; margin-left: 502px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">RoboteqDriver</font></div></div></div></foreignObject><text x="576" y="797" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">RoboteqDriver</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-31"><g><rect x="507.25" y="980.72" width="137.5" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 136px; height: 1px; padding-top: 1011px; margin-left: 508px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">CAN</div></div></div></foreignObject><text x="576" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">CAN</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-32"><g><path d="M 576 919.43 L 576 962.01" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 576 907.43 L 582 919.43 L 570 919.43 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 576 974.01 L 570 962.01 L 582 962.01 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-35"><g><path d="M 1015 919.43 L 1015 962.01" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1015 907.43 L 1021 919.43 L 1009 919.43 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 1015 974.01 L 1009 962.01 L 1021 962.01 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-38"><g><rect x="715" y="763.22" width="160" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 790px; margin-left: 716px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">EStopManager</font></div></div></div></foreignObject><text x="795" y="796" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">EStopManager</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-56"><g><path d="M 796.78 743.27 L 796.78 750.87" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 796.78 734.27 L 801.28 743.27 L 792.28 743.27 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 796.78 759.87 L 792.28 750.87 L 801.28 750.87 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-58"><g><path d="M 796.79 825.57 L 796.83 838.37" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 796.75 816.57 L 801.29 825.56 L 792.29 825.59 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 796.87 847.37 L 792.33 838.38 L 801.33 838.35 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-63"><g><path d="M 632.65 705.72 L 586 705.7 Q 576 705.7 576 715.7 L 576 753.37" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 641.65 705.72 L 632.64 710.22 L 632.65 701.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 576 762.37 L 571.5 753.37 L 580.5 753.37 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-64"><g><path d="M 576 825.57 L 576 838.37" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 576 816.57 L 580.5 825.57 L 571.5 825.57 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 576 847.37 L 571.5 838.37 L 580.5 838.37 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-66"><g><path d="M 947.65 775.72 L 887.35 775.72" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 956.65 775.72 L 947.65 780.22 L 947.65 771.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 878.35 775.72 L 887.35 771.22 L 887.35 780.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-73"><g><path d="M 947.65 753.22 L 907.5 753.2 Q 897.5 753.2 897.5 743.2 L 897.5 715.7 Q 897.5 705.7 887.5 705.7 L 847.35 705.72" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 956.65 753.22 L 947.64 757.72 L 947.65 748.72 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 838.35 705.72 L 847.35 701.22 L 847.36 710.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-74"><g><path d="M 947.65 798.21 L 928.5 798.2 Q 918.5 798.2 918.5 808.2 L 918.5 853.2 Q 918.5 863.2 908.5 863.2 L 889.23 863.21" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 956.65 798.22 L 947.64 802.71 L 947.65 793.71 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 880.23 863.22 L 889.23 858.71 L 889.24 867.71 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-75"><g><rect x="1357.36" y="654.84" width="190" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 702px; margin-left: 1358px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">battery_driver</font></div></div></div></foreignObject><text x="1452" y="708" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">battery_driver</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-77"><g><path d="M 1452.36 980.72 L 1452.36 763.55" fill="none" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1452.36 751.55 L 1458.36 763.55 L 1446.36 763.55 Z" fill="#888a85" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-79"><g><rect x="950" y="980.72" width="130" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 1011px; margin-left: 951px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">RPi GPIO</div></div></div></foreignObject><text x="1015" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">RPi GPIO</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-80"><g><rect x="1383.61" y="980.72" width="137.5" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 136px; height: 1px; padding-top: 1011px; margin-left: 1385px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">ADC</div></div></div></foreignObject><text x="1452" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">ADC</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-84"><g><rect x="195" y="484.72" width="180" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 517px; margin-left: 196px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">imu_broadcaster</font></div></div></div></foreignObject><text x="285" y="522" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">imu_broadcaster</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-85"><g><rect x="490" y="347" width="240" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 379px; margin-left: 491px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">joint_state_broadcaster</font></div></div></div></foreignObject><text x="610" y="385" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">joint_state_broadcaster</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-86"><g><rect x="370" y="424.72" width="180" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 457px; margin-left: 371px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">drive_controller</font></div></div></div></foreignObject><text x="460" y="462" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">drive_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-95"><g><rect x="32.5" y="422.5" width="220" height="55" rx="8.25" ry="8.25" fill="#ed3b42" stroke="none" transform="rotate(270,142.5,450)" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 144.5 450)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 218px; height: 1px; padding-top: 450px; margin-left: 36px;"><div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><div style="text-align: left;"><span style="background-color: initial;"><font style="font-size: 18px;">controller_manager</font></span></div></div></div></div></foreignObject><text x="145" y="456" fill="#FFFFFF" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">controller_manager</text></switch></g></g></g><g data-cell-id="daj83a5Ns2Ukew12XzJU-1"><g><path d="M 285 620.72 L 285 551.09" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 285 545.84 L 288.5 552.84 L 285 551.09 L 281.5 552.84 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="daj83a5Ns2Ukew12XzJU-2"><g><path d="M 285 620.72 L 285 551.09" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 285 545.84 L 288.5 552.84 L 285 551.09 L 281.5 552.84 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-99"><g><rect x="160" y="620.72" width="250" height="311.5" rx="15" ry="15" fill="#cccccc" stroke="#e6e6e6" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 248px; height: 1px; padding-top: 632px; margin-left: 161px;"><div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><div style="font-size: 24px;"><font style="font-size: 24px;" color="#1a1a1a">PantherImuSensor</font></div><div style="font-size: 24px;"><br /></div></div></div></div></foreignObject><text x="285" y="651" fill="#FFFFFF" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">PantherImuSensor&#xa;</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-103"><g><rect x="216.25" y="980.72" width="137.5" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 136px; height: 1px; padding-top: 1011px; margin-left: 217px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">IMU</div></div></div></foreignObject><text x="285" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">IMU</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-106"><g><path d="M 285 563.43 L 285 602.01" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 285 551.43 L 291 563.43 L 279 563.43 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 285 614.01 L 279 602.01 L 291 602.01 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-116"><g><path d="M 655.22 424.02 L 658.41 602.02" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 655 412.03 L 661.22 423.92 L 649.22 424.13 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 658.63 614.01 L 652.41 602.12 L 664.41 601.91 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-117"><g><path d="M 530.24 504.93 L 531.39 604.51" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 530.1 492.93 L 536.24 504.86 L 524.24 505 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 531.53 616.5 L 525.39 604.57 L 537.39 604.44 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-118"><g><rect x="295" y="185.72" width="130" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 218px; margin-left: 296px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">ekf_filter</font></div></div></div></foreignObject><text x="360" y="223" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">ekf_filter</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-120"><g><rect x="180" y="705.22" width="210" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 208px; height: 1px; padding-top: 732px; margin-left: 181px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">ImuFilter</font></div></div></div></foreignObject><text x="285" y="738" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">ImuFilter</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-121"><g><rect x="190" y="831.34" width="190" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 858px; margin-left: 191px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">Spatial</font></div></div></div></foreignObject><text x="285" y="864" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">Spatial</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-123"><g><path d="M 284.5 980.22 L 284.91 900.05" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 284.97 888.05 L 290.91 900.08 L 278.91 900.02 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-124"><g><path d="M 284.66 831.34 L 284.92 773.93" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 284.97 761.93 L 290.92 773.95 L 278.92 773.9 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-125"><g><path d="M 240 484.72 L 240 257.9 Q 240 247.9 240 237.9 L 240 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 240 14.47 L 245 24.47 L 240 21.97 L 235 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 223.05934473723292 38.25198888361385)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 38px; margin-left: 223px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>imu/data</b></div></div></div></foreignObject><text x="223" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">imu/data</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-129"><g><path d="M 392.32 425.08 L 392.49 257.69" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 392.5 250.19 L 397.49 260.2 L 392.49 257.69 L 387.49 260.19 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 379.32602896003704 418.0649495053864)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 1px; height: 1px; padding-top: 418px; margin-left: 379px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b><font style="font-size: 18px;">odometry/wheels</font></b></div></div></div></foreignObject><text x="379" y="423" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px">odometry/wheels</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-130"><g><path d="M 327.5 185.72 L 327.97 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 327.99 14.47 L 332.96 24.49 L 327.97 21.97 L 322.96 24.46 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 315.918317869348 38.69214084552837)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 316px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>odometry/filtered</b></div></div></div></foreignObject><text x="316" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">odometry/filtered</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-131"><g><path d="M 240 484.72 L 240 225.7 Q 240 215.7 250 215.7 L 283.03 215.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 290.53 215.72 L 280.53 220.71 L 283.03 215.72 L 280.53 210.71 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-134"><g><path d="M 631.12 347.78 L 631.1 189 Q 631.1 179 631.03 169 L 630.08 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 630.03 14.47 L 635.09 24.44 L 630.08 21.97 L 625.09 24.5 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 618.1017614886418 44.4101760279836)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 44px; margin-left: 618px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>joint_states</b></div></div></div></foreignObject><text x="618" y="50" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">joint_states</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-135"><g><path d="M 470 10 L 470.48 207.9 Q 470.5 217.9 470.5 227.9 L 470.44 418.03" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 470.44 425.53 L 465.44 415.53 L 470.44 418.03 L 475.44 415.53 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 453.0721932435777 40.03598817157367)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 453px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>cmd_vel</b></div></div></div></foreignObject><text x="453" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">cmd_vel</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-136"><g><path d="M 582.16 347.12 L 582.2 189 Q 582.2 179 582.13 169 L 581.09 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 581.03 14.47 L 586.1 24.44 L 581.09 21.97 L 576.1 24.51 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 569.0971461347985 42.85018642894648)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 43px; margin-left: 569px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>dynamic_joint_states</b></div></div></div></foreignObject><text x="569" y="48" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">dynamic_joint_states</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-138"><g><path d="M 1209.66 730.54 L 1209.7 380.9 Q 1209.7 370.9 1209.71 360.9 L 1209.99 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1210 14.47 L 1214.99 24.48 L 1209.99 21.97 L 1204.99 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1198.040658419977 39.35417466263085)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 1198px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;"> hardware/e_stop</b></div></div></div></foreignObject><text x="1198" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end"> hardware/e_stop</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-140"><g><path d="M 1139.55 732.43 L 1139.6 380.9 Q 1139.6 370.9 1139.61 360.9 L 1139.99 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1140 14.47 L 1144.98 24.48 L 1139.99 21.97 L 1134.98 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1128.0420299914413 40.34417494560757)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1128px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>hardware/robot_driver_state</b></div></div></div></foreignObject><text x="1128" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/robot_driver_state</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-143"><g><path d="M 1173.75 730.72 L 1173.8 380.9 Q 1173.8 370.9 1173.81 360.9 L 1173.99 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1174 14.47 L 1178.99 24.47 L 1173.99 21.97 L 1168.99 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1162.0407692556278 39.434175835366155)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 1162px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;"> hardware/io_state</b></div></div></div></foreignObject><text x="1162" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end"> hardware/io_state</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-144"><g><path d="M 980.23 721.07 L 980.2 380.9 Q 980.2 370.9 980.19 360.9 L 980 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 980.23 728.57 L 975.23 718.57 L 980.23 721.07 L 985.23 718.57 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 967.0417251957298 40.1241734010298)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 967px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;">hardware/</b><b>e_stop_trigger</b></div></div></div></foreignObject><text x="967" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/e_stop_trigger</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-145"><g><path d="M 1020.7 720.62 L 1020.7 380.9 Q 1020.7 370.9 1020.68 360.9 L 1020 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1020.7 728.12 L 1015.7 718.12 L 1020.7 720.62 L 1025.7 718.12 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1007.0409632102225 39.57417226182657)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1007px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;">hardware/</b><b>e_stop_reset</b></div></div></div></foreignObject><text x="1007" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/e_stop_reset</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-148"><g><path d="M 1060.6 723.59 L 1060.6 380.9 Q 1060.6 370.9 1060.58 360.9 L 1060 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1060.6 731.09 L 1055.6 721.09 L 1060.6 723.59 L 1065.6 721.09 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1047.0407831051025 39.444172386302576)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 1047px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;">hardware/</b><b><i>&lt;hw_component&gt;</i>_enable</b></div></div></div></foreignObject><text x="1047" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/&lt;hw_component&gt;_enable</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-149"><g><rect x="665" y="210" width="240" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 242px; margin-left: 666px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">robot_state_publisher</font></div></div></div></foreignObject><text x="785" y="248" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">robot_state_publisher</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-150"><g><path d="M 830.01 210 L 830 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 830 14.47 L 835 24.47 L 830 21.97 L 825 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 818.0015500125105 41.00050021124936)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 41px; margin-left: 818px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>robot_description</b></div></div></div></foreignObject><text x="818" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">robot_description</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-151"><g><path d="M 740 210 L 740 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 740 14.47 L 745 24.47 L 740 21.97 L 735 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 728 41)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 41px; margin-left: 728px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf</b></div></div></div></foreignObject><text x="728" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-152"><g><path d="M 785 210 L 785 120.5 Q 785 110.5 785 100.5 L 785 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 785 14.47 L 790 24.47 L 785 21.97 L 780 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 771.1618442375116 41.560542301394726)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 42px; margin-left: 771px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf_static</b></div></div></div></foreignObject><text x="771" y="47" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf_static</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-153"><g><path d="M 360 185.72 L 360 108.4 Q 360 98.4 360 88.4 L 360 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 360 14.47 L 365 24.47 L 360 21.97 L 355 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 348.1597807831874 38.27752278433415)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 38px; margin-left: 348px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf</b></div></div></div></foreignObject><text x="348" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-156"><g><path d="M 1547.36 677.34 L 1600.5 677.31 Q 1610.5 677.3 1610.49 667.3 L 1610.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1610 14.47 L 1615.01 24.47 L 1610.01 21.97 L 1605.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1598.0228112826962 40.44768432687242)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1598px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">battery/battery_status</b></div></div></div></foreignObject><text x="1598" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">battery/battery_status</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-158"><g><path d="M 1173.75 730.72 L 1173.79 690 Q 1173.8 680 1183.8 680 L 1345.39 680" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1352.89 680 L 1342.89 685 L 1345.39 680 L 1342.89 675 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-159"><g><rect x="1720" y="980.72" width="160" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 1011px; margin-left: 1721px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><div style="font-size: 20px;"><span style="">Bumper Lights</span><br /></div></div></div></div></foreignObject><text x="1800" y="1016" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="16px" text-anchor="middle" font-weight="bold">Bumper Lights&#xa;</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-160"><g><rect x="1710" y="731.47" width="180" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 778px; margin-left: 1711px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">lights_driver</font></div></div></div></foreignObject><text x="1800" y="784" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">lights_driver</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-161"><g><rect x="1710" y="430" width="180" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 477px; margin-left: 1711px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">lights_controller</font></div></div></div></foreignObject><text x="1800" y="483" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">lights_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-162"><g><path d="M 1800 821.47 L 1800 962.01" fill="none" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1800 974.01 L 1794 962.01 L 1806 962.01 Z" fill="#888a85" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-165"><g><path d="M 1800 520 L 1800 719.5" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1800 727 L 1795 717 L 1800 719.5 L 1805 717 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1782 701)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 1px; height: 1px; padding-top: 701px; margin-left: 1782px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="font-size: 16px;"><font style="font-size: 16px;">lights/channel_1_frame</font></b></div></div></div></foreignObject><text x="1782" y="706" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="16px">lights/channel_1_frame</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-167"><g><path d="M 1845 520 L 1845 719.5" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1845 727 L 1840 717 L 1845 719.5 L 1850 717 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1827 701)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 1px; height: 1px; padding-top: 701px; margin-left: 1827px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="font-size: 16px;"><font style="font-size: 16px;">lights/channel_2_frame</font></b></div></div></div></foreignObject><text x="1827" y="706" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="16px">lights/channel_2_frame</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-168"><g><path d="M 1700.26 497.5 L 1502 497.5" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1707.76 497.5 L 1697.76 502.5 L 1700.26 497.5 L 1697.76 492.5 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 510px; margin-left: 1687px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: center; font-size: 18px;"><span style="background-color: initial; font-size: 18px;"><b style="font-size: 18px;"><font style="font-size: 18px;">lights/</font></b></span><span style="background-color: initial; text-align: right; font-size: 18px;"><b style="font-size: 18px;">set_animation</b></span></div></div></div></div></foreignObject><text x="1687" y="515" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">lights/set_animation</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-170"><g><path d="M 1899.74 753.98 L 1910.5 753.99 Q 1920.5 754 1920.49 744 L 1920 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1892.24 753.97 L 1902.24 748.98 L 1899.74 753.98 L 1902.23 758.98 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1907.0184835596563 37.50689698330439)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 38px; margin-left: 1907px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: center;"><b>lights/set_brightness</b><br /></div></div></div></div></foreignObject><text x="1907" y="43" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">lights/set_brightness&#xa;</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-173"><g><rect x="1312" y="314.72" width="190" height="100" rx="15" ry="15" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 367px; margin-left: 1313px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">safety_manager</font></div></div></div></foreignObject><text x="1407" y="372" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">safety_manager</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-174"><g><rect x="1393.35" y="150.57" width="180" height="70" rx="10.5" ry="10.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 188px; margin-left: 1394px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">system_monitor</font></div></div></div></foreignObject><text x="1483" y="193" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">system_monitor</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-183"><g><path d="M 1393.35 185.57 L 1350 185.59 Q 1340 185.6 1340 175.6 L 1340 150 Q 1340 140 1340 130 L 1340 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1340 14.47 L 1345 24.47 L 1340 21.97 L 1335 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1328 40.95000843486335)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 41px; margin-left: 1328px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>system_status</b></div></div></div></foreignObject><text x="1328" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">system_status</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-184"><g><path d="M 1393.35 185.57 L 1349.6 185.59 Q 1339.6 185.6 1339.6 195.6 L 1339.55 303.75" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1339.55 311.25 L 1334.56 301.25 L 1339.55 303.75 L 1344.56 301.25 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-187"><g><path d="M 1209.66 729.64 L 1209.7 374.7 Q 1209.7 364.7 1219.7 364.7 L 1300.03 364.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 364.72 L 1297.53 369.72 L 1300.03 364.72 L 1297.53 359.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-189"><g><path d="M 1173.75 730.72 L 1173.8 349.7 Q 1173.8 339.7 1183.8 339.7 L 1300.03 339.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 339.72 L 1297.53 344.72 L 1300.03 339.72 L 1297.53 334.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-190"><g><path d="M 1140 730 L 1140 324.7 Q 1140 314.7 1150 314.7 L 1300.03 314.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 314.72 L 1297.53 319.72 L 1300.03 314.72 L 1297.53 309.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-191"><g><path d="M 1060.6 723.05 L 1060.6 424.7 Q 1060.6 414.7 1070.6 414.7 L 1312 414.72" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1060.6 730.55 L 1055.6 720.55 L 1060.6 723.05 L 1065.6 720.55 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-195"><g><path d="M 1547.36 677.34 L 1556.68 677.32 Q 1566 677.3 1576 677.46 L 1600 677.84 Q 1610 678 1610 668 L 1610 374.7 Q 1610 364.7 1600 364.7 L 1513.97 364.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1506.47 364.72 L 1516.47 359.72 L 1513.97 364.72 L 1516.47 369.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-204"><g><path d="M 631.6 347.42 L 631.6 320 Q 631.6 310 631.66 300 L 631.94 250 Q 632 240 642 240 L 653.03 240" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 660.53 240 L 650.53 245 L 653.03 240 L 650.53 235 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-214"><g><path d="M 20 230.3 L 80.03 230.26" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-215"><g><path d="M 20 189.84 L 80 189.75" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/></g></g><g data-cell-id="D_R0GpuGBRVuPwS73e86-2"><g><rect x="1312" y="430" width="190" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 477px; margin-left: 1313px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">lights_manager</font></div></div></div></foreignObject><text x="1407" y="483" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">lights_manager</text></switch></g></g></g><g data-cell-id="D_R0GpuGBRVuPwS73e86-7"><g><path d="M 1547.36 677.34 L 1600 677.31 Q 1610 677.3 1610 667.3 L 1610 462.5 Q 1610 452.5 1600 452.5 L 1513.97 452.5" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1506.47 452.5 L 1516.47 447.5 L 1513.97 452.5 L 1516.47 457.5 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="D_R0GpuGBRVuPwS73e86-9"><g><path d="M 1209.66 731.08 L 1209.7 485 Q 1209.7 475 1219.7 475 L 1300.03 475" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 475 L 1297.53 480 L 1300.03 475 L 1297.53 470 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-3"><g><path d="M 1254.74 775.72 L 1467.5 775.7 Q 1477.5 775.7 1487.5 775.73 L 1710 776.47" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1247.24 775.72 L 1257.24 770.72 L 1254.74 775.72 L 1257.24 780.72 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 790px; margin-left: 1687px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><span style="forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><b style="forced-color-adjust: none;"><font style="forced-color-adjust: none; font-size: 16px;">hardware/</font></b></span><span style="forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; font-size: 16px;"><b style="forced-color-adjust: none;">led_control_enable</b></span></div></div></div></foreignObject><text x="1687" y="796" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/led_control_enable</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-6"><g><path d="M 440.02 425.38 L 440 227.9 Q 440 217.9 440 207.9 L 440 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 440 14.47 L 445 24.47 L 440 21.97 L 435 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 421.0719118764564 39.92140072145239)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 421px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf</b></div></div></div></foreignObject><text x="421" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-8"><g><path d="M 1547.36 699.84 L 1630.5 699.8 Q 1640.5 699.8 1640.49 689.8 L 1640.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1640 14.47 L 1645.01 24.47 L 1640.01 21.97 L 1635.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1628.0231543966713 41.947429886303745)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 42px; margin-left: 1628px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">battery/</b><b>charging_status</b></div></div></div></foreignObject><text x="1628" y="47" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">battery/charging_status</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-14"><g><path d="M 980.23 722.06 L 980.2 399.7 Q 980.2 389.7 990.2 389.7 L 1312 389.72" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 980.23 729.56 L 975.23 719.56 L 980.23 722.06 L 985.23 719.56 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-15"><g><rect x="950" y="850.72" width="130" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 878px; margin-left: 951px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">GPIODriver</font></div></div></div></foreignObject><text x="1015" y="883" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">GPIODriver</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-17"><g><path d="M 937.65 888.22 L 889.23 888.22" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 946.65 888.22 L 937.65 892.72 L 937.65 883.72 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 880.23 888.22 L 889.23 883.72 L 889.23 892.72 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-2"><g><path d="M 392.5 185.72 L 392.97 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 392.99 14.47 L 397.96 24.49 L 392.97 21.97 L 387.96 24.46 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 380.918317869348 38.69214084552837)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 381px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>diagnostics</b></div></div></div></foreignObject><text x="381" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-3"><g><path d="M 1547.36 722.34 L 1660.5 722.3 Q 1670.5 722.3 1670.49 712.3 L 1670.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1670 14.47 L 1675.01 24.47 L 1670.01 21.97 L 1665.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1658.02347584129 43.447193258440564)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 43px; margin-left: 1658px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: left;"><span style="background-color: initial;"><b>diagnostics</b></span></div></div></div></div></foreignObject><text x="1658" y="49" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-4"><g><path d="M 1393.35 168.07 L 1380.5 168.09 Q 1370.5 168.1 1370.47 158.1 L 1370.04 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1370.01 14.47 L 1375.05 24.46 L 1370.04 21.97 L 1365.05 24.49 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1358.0947706300105 39.98228594656824)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1358px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">diagnostics</b></div></div></div></foreignObject><text x="1358" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-6"><g><path d="M 1710 753.97 L 1680.5 753.99 Q 1670.5 754 1670.49 744 L 1670.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1670 14.47 L 1675.01 24.47 L 1670.01 21.97 L 1665.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1658.021171728833 41.506892718330505)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 42px; margin-left: 1658px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: left;"><br /></div></div></div></div></foreignObject><text x="1658" y="47" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">&#xa;</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-7"><g><path d="M 1102.5 730.72 L 1102.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1102 14.47 L 1107.01 24.47 L 1102.01 21.97 L 1097.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1090.0206207924284 39.727103790911734)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1090px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">diagnostics</b></div></div></div></foreignObject><text x="1090" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-8"><g><rect x="20" y="55" width="60" height="30" rx="4.5" ry="4.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-11"><g><rect x="0" y="5.15" width="100" height="40" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 25px; margin-left: 50px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: nowrap;"><span style="font-size: 20px; font-weight: 700; text-wrap: wrap;"><font color="#888a85">Legend:</font></span></div></div></div></foreignObject><text x="50" y="30" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="16px" text-anchor="middle">Legend:</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-12"><g><rect x="20" y="95" width="60" height="30" rx="4.5" ry="4.5" fill="#cccccc" stroke="#e6e6e6" stroke-width="6" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-14"><g><rect x="20" y="135" width="60" height="30" rx="4.5" ry="4.5" fill="#ffffff" stroke="#b0b3ac" stroke-width="2" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-18"><g><rect x="95" y="55" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 62px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Nodes</font></b></div></div></div></foreignObject><text x="97" y="80" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Nodes</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-19"><g><rect x="95" y="95" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 102px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Plugin</font></b></div></div></div></foreignObject><text x="97" y="120" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Plugin</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-20"><g><rect x="95" y="135" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 142px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Objects</font></b></div></div></div></foreignObject><text x="97" y="160" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Objects</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-21"><g><rect x="95" y="175" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 182px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Topics</font></b></div></div></div></foreignObject><text x="97" y="200" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Topics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-22"><g><rect x="95" y="215" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 222px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Services</font></b></div></div></div></foreignObject><text x="97" y="240" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Services</text></switch></g></g></g><g data-cell-id="Y5d-tTOP6FVwuIk47fc7-3"><g><path d="M 20 270 L 61.29 269.7" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 73.29 269.62 L 61.34 275.7 L 61.25 263.7 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="Y5d-tTOP6FVwuIk47fc7-4"><g><rect x="95" y="255" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 262px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Data Transfer</font></b></div></div></div></foreignObject><text x="97" y="280" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Data Trans...</text></switch></g></g></g></g></g></g><switch><g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/><a transform="translate(0,-5)" xlink:href="https://www.drawio.com/doc/faq/svg-export-text-problems" target="_blank"><text text-anchor="middle" font-size="10px" x="50%" y="100%">Text is not SVG - cannot display</text></a></switch></svg>
diff --git a/.docs/ros2_system_design.drawio.svg b/.docs/ros2_system_design.drawio.svg
new file mode 100644
index 00000000..ef55b4d4
--- /dev/null
+++ b/.docs/ros2_system_design.drawio.svg
@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than draw.io -->
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="1932px" height="1041px" viewBox="-0.5 -0.5 1932 1041" content="&lt;mxfile host=&quot;app.diagrams.net&quot; agent=&quot;Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/124.0.0.0 Safari/537.36&quot; scale=&quot;1&quot; border=&quot;0&quot; version=&quot;24.7.6&quot;&gt;&#10;  &lt;diagram id=&quot;5C2LWovSXbSQWriEvJtN&quot; name=&quot;Page-1&quot;&gt;&#10;    &lt;mxGraphModel dx=&quot;1350&quot; dy=&quot;1328&quot; grid=&quot;1&quot; gridSize=&quot;10&quot; guides=&quot;1&quot; tooltips=&quot;1&quot; connect=&quot;1&quot; arrows=&quot;1&quot; fold=&quot;1&quot; page=&quot;0&quot; pageScale=&quot;1&quot; pageWidth=&quot;850&quot; pageHeight=&quot;1100&quot; math=&quot;0&quot; shadow=&quot;0&quot;&gt;&#10;      &lt;root&gt;&#10;        &lt;mxCell id=&quot;0&quot; /&gt;&#10;        &lt;mxCell id=&quot;1&quot; parent=&quot;0&quot; /&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-23&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;husarion_ugv_localization&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;imageHeight=24;arcSize=6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;970&quot; y=&quot;165.15&quot; width=&quot;180&quot; height=&quot;100&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-179&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;husarion_ugv_controller&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=#FFFFFF;strokeWidth=6;strokeColor=#E6E6E6;imageHeight=24;arcSize=6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;840&quot; y=&quot;320&quot; width=&quot;600&quot; height=&quot;240&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-199&quot; value=&quot;&amp;lt;span style=&amp;quot;font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;husarion_ugv_lights&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=top;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2400&quot; y=&quot;380&quot; width=&quot;200&quot; height=&quot;450&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-202&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;husarion_ugv_hardware_interfaces&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=5;strokeColor=#E6E6E6;imageHeight=24;arcSize=6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;840&quot; y=&quot;590&quot; width=&quot;1150&quot; height=&quot;360.12&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-206&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;husarion_ugv_battery&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=top;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2027.36&quot; y=&quot;610&quot; width=&quot;256&quot; height=&quot;140&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-208&quot; value=&quot;&amp;lt;span style=&amp;quot;font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;husarion_ugv_manager&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=bottom;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1978.87&quot; y=&quot;270&quot; width=&quot;243&quot; height=&quot;277.55&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-211&quot; value=&quot;&amp;lt;span style=&amp;quot;font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot;&amp;gt;husarion_ugv_diagnostics&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fillColor=default;strokeWidth=6;strokeColor=#E6E6E6;opacity=90;verticalAlign=top;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2083.36&quot; y=&quot;110&quot; width=&quot;200&quot; height=&quot;120&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-24&quot; value=&quot;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot; style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;{Robot}System&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;align=center;labelBackgroundColor=none;fillColor=#CCCCCC;strokeColor=#E6E6E6;arcSize=6;verticalAlign=top;fontColor=#FFFFFF;gradientColor=none;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1155&quot; y=&quot;610.72&quot; width=&quot;815&quot; height=&quot;311.5&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; value=&quot;&amp;lt;span style=&amp;quot;color: rgb(255, 255, 255); text-align: left;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;hardware_controller&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ED3B42;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1660&quot; y=&quot;720.72&quot; width=&quot;285&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;GPIOController&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1416.88&quot; y=&quot;840.72&quot; width=&quot;160&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-28&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;MotorsController&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1345&quot; y=&quot;670.72&quot; width=&quot;190&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-29&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;CANopenController&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1176&quot; y=&quot;840.72&quot; width=&quot;200&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-30&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;RoboteqDriver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1201&quot; y=&quot;755.72&quot; width=&quot;150&quot; height=&quot;47.5&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-31&quot; value=&quot;CAN&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1207.25&quot; y=&quot;970.72&quot; width=&quot;137.5&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-32&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;labelBackgroundColor=none;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=16;fontColor=#888a85;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;fillColor=#FFFFFF;startArrow=block;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-29&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-31&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1204.25&quot; y=&quot;1210.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1291&quot; y=&quot;940.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-35&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=1;exitDx=0;exitDy=0;fillColor=#FFFFFF;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startArrow=block;endFill=1;&quot; parent=&quot;1&quot; source=&quot;NPlMt0osOJUE4kA7_BK4-15&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-79&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1501.13&quot; y=&quot;871.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1496.88&quot; y=&quot;970.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-38&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;EStopManager&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1415&quot; y=&quot;753.22&quot; width=&quot;160&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-56&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.488;entryY=-0.004;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.789;exitY=1;exitDx=0;exitDy=0;exitPerimeter=0;entryPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1496.78&quot; y=&quot;720.9200000000001&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1496.78&quot; y=&quot;753.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-58&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.789;exitY=1;exitDx=0;exitDy=0;exitPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1496.74&quot; y=&quot;803.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1496.74&quot; y=&quot;835.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-63&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.5;exitDx=0;exitDy=0;startFill=1;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-28&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-30&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1255&quot; y=&quot;688.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1255&quot; y=&quot;720.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-64&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;startFill=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-30&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-29&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1290&quot; y=&quot;810.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1291.34&quot; y=&quot;840.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-66&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.25;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;edgeStyle=orthogonalEdgeStyle;exitX=0;exitY=0.5;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-38&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1660&quot; y=&quot;766&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1593&quot; y=&quot;875.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-73&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.5;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.25;exitDx=0;exitDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-28&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1626&quot; y=&quot;695.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1545&quot; y=&quot;696.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-74&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.25;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.75;exitDx=0;exitDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1680&quot; y=&quot;797.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1599&quot; y=&quot;798.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;battery_driver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2057.36&quot; y=&quot;644.84&quot; width=&quot;190&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-77&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#888a85;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=0;exitDx=0;exitDy=0;fillColor=#FFFFFF;entryX=0.5;entryY=1;entryDx=0;entryDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-80&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2509.86&quot; y=&quot;1146.53&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2133.9400000000005&quot; y=&quot;720.7200000000001&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-79&quot; value=&quot;RPi GPIO&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1650&quot; y=&quot;970.72&quot; width=&quot;130&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-80&quot; value=&quot;ADC&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2083.61&quot; y=&quot;970.7199999999999&quot; width=&quot;137.5&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;imu_broadcaster&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;895&quot; y=&quot;474.72&quot; width=&quot;180&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;joint_state_broadcaster&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1190&quot; y=&quot;337.00000000000006&quot; width=&quot;240&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;drive_controller&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1070&quot; y=&quot;414.72&quot; width=&quot;180&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-95&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: left;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;controller_manager&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#FFFFFF;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;direction=north;horizontal=0;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;815&quot; y=&quot;330&quot; width=&quot;55&quot; height=&quot;220&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;daj83a5Ns2Ukew12XzJU-1&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;&quot; edge=&quot;1&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot;&gt;&#10;          &lt;mxGeometry relative=&quot;1&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;daj83a5Ns2Ukew12XzJU-2&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;&quot; edge=&quot;1&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot;&gt;&#10;          &lt;mxGeometry relative=&quot;1&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; value=&quot;&amp;lt;div style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#1a1a1a&amp;quot; style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;PhidgetImuSensor&amp;lt;/font&amp;gt;&amp;lt;/div&amp;gt;&amp;lt;div style=&amp;quot;font-size: 24px;&amp;quot;&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;align=center;labelBackgroundColor=none;fillColor=#CCCCCC;strokeColor=#E6E6E6;fontColor=#FFFFFF;arcSize=6;verticalAlign=top;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;860&quot; y=&quot;610.72&quot; width=&quot;250&quot; height=&quot;311.5&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-103&quot; value=&quot;IMU&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;916.25&quot; y=&quot;970.72&quot; width=&quot;137.5&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-106&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.5;exitY=1;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-99&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;660&quot; y=&quot;1060.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;721.25&quot; y=&quot;1060.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-116&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.25;entryY=0;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.687;exitY=0.972;exitDx=0;exitDy=0;exitPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-24&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1365&quot; y=&quot;380&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1365&quot; y=&quot;610.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-117&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=0.094;entryY=0.008;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0.889;exitY=1.025;exitDx=0;exitDy=0;entryPerimeter=0;exitPerimeter=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-24&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1235&quot; y=&quot;480.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1235&quot; y=&quot;610.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;ekf_filter&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;995&quot; y=&quot;175.72&quot; width=&quot;130&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-120&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;ImuFilter&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;880&quot; y=&quot;695.22&quot; width=&quot;210&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-121&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;Spatial&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;890&quot; y=&quot;821.34&quot; width=&quot;190&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-123&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=0;exitDx=0;exitDy=0;entryX=0.5;entryY=1;entryDx=0;entryDy=0;fillColor=default;endFill=1;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-121&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;984.5&quot; y=&quot;970.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;984.5&quot; y=&quot;922.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-124&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;exitX=0.5;exitY=0;exitDx=0;exitDy=0;entryX=0.5;entryY=1;entryDx=0;entryDy=0;fillColor=default;endFill=1;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-120&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;984.6600000000001&quot; y=&quot;821.34&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;984.6600000000001&quot; y=&quot;773.34&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-125&quot; value=&quot;&amp;lt;b&amp;gt;imu/data&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.25;exitY=0;exitDx=0;exitDy=0;align=right;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.8832&quot; y=&quot;15&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;940&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;975.47&quot; y=&quot;511.44000000000005&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-129&quot; value=&quot;&amp;lt;b&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;odometry/wheels&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;entryX=0.75;entryY=1;entryDx=0;entryDy=0;align=left;exitX=0.124;exitY=0.006;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.9376&quot; y=&quot;15&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1085&quot; y=&quot;305.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1085&quot; y=&quot;410.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-130&quot; value=&quot;&amp;lt;b&amp;gt;odometry/filtered&amp;lt;/b&amp;gt;&quot; style=&quot;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.25;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6758&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1028&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1040.3&quot; y=&quot;190.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-131&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.25;exitY=0;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-84&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.0769&quot; y=&quot;18&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;965&quot; y=&quot;40.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;965.47&quot; y=&quot;510.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-134&quot; value=&quot;&amp;lt;b&amp;gt;joint_states&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.588;exitY=0.013;exitDx=0;exitDy=0;align=right;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7994&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1330&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1330.0100000000002&quot; y=&quot;309.09000000000015&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-135&quot; value=&quot;&amp;lt;b&amp;gt;cmd_vel&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0.558;entryY=0.088;entryDx=0;entryDy=0;entryPerimeter=0;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.8578&quot; y=&quot;-15&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1170&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1170&quot; y=&quot;400&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-136&quot; value=&quot;&amp;lt;b&amp;gt;dynamic_joint_states&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.384;exitY=0.002;exitDx=0;exitDy=0;align=right;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.8069&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1281&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1280.0100000000002&quot; y=&quot;309.09000000000015&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-138&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;&amp;amp;nbsp;hardware/e_stop&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.876;exitY=-0.002;exitDx=0;exitDy=0;align=right;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; y=&quot;735.0912500000003&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-140&quot; value=&quot;&amp;lt;b&amp;gt;hardware/robot_driver_state&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.63;exitY=0.019;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1840&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1830&quot; y=&quot;710&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-143&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;&amp;amp;nbsp;hardware/io_state&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.75;exitY=0;exitDx=0;exitDy=0;align=right;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1874&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1870&quot; y=&quot;735.09&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-144&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;hardware/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;e_stop_trigger&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.071;exitY=0.001;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1680&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1680.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-145&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;hardware/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;e_stop_reset&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.213;exitY=-0.004;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1720&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1720.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-148&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: center;&amp;quot;&amp;gt;hardware/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;&amp;lt;i&amp;gt;&amp;amp;lt;hw_component&amp;amp;gt;&amp;lt;/i&amp;gt;_enable&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.353;exitY=0.029;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1760&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1760.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-149&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;robot_state_publisher&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1365&quot; y=&quot;200&quot; width=&quot;240&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-150&quot; value=&quot;&amp;lt;b&amp;gt;robot_description&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.673;exitY=-0.008;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6903&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1530&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1530.01&quot; y=&quot;199.99999999999997&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-151&quot; value=&quot;&amp;lt;b&amp;gt;/tf&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6915&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1440&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1440&quot; y=&quot;200&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-152&quot; value=&quot;&amp;lt;b&amp;gt;/tf_static&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.5;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-149&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.68&quot; y=&quot;12&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1485&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1482.21&quot; y=&quot;220.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-153&quot; value=&quot;&amp;lt;b&amp;gt;/tf&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.5;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6747&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1060&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1070&quot; y=&quot;190.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-156&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;battery/battery_status&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=1;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9179&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2310&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2230&quot; y=&quot;670&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-158&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;jumpStyle=none;jumpSize=6;exitX=0.75;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2275&quot; y=&quot;530.72&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; y=&quot;730&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1874&quot; y=&quot;670&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-159&quot; value=&quot;&amp;lt;div style=&amp;quot;font-size: 20px;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;&amp;quot;&amp;gt;Bumper Lights&amp;lt;/span&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;strokeWidth=8;fontFamily=Helvetica;fontSize=16;fontColor=#ffffff;fillColor=#434343;gradientColor=none;fontStyle=1;glass=0;shadow=0;spacingTop=1;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2420&quot; y=&quot;970.7200000000001&quot; width=&quot;160&quot; height=&quot;60&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights_driver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2410&quot; y=&quot;721.47&quot; width=&quot;180&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights_controller&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2410&quot; y=&quot;420&quot; width=&quot;180&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-162&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#888a85;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;fillColor=#FFFFFF;entryX=0.5;entryY=0;entryDx=0;entryDy=0;startArrow=none;startFill=0;exitX=0.5;exitY=1;exitDx=0;exitDy=0;endFill=1;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-159&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2243.75&quot; y=&quot;915.8200000000002&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2243.41&quot; y=&quot;966.3200000000002&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-165&quot; value=&quot;&amp;lt;b style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;lights/channel_1_frame&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=16;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=left;entryX=0.5;entryY=0;entryDx=0;entryDy=0;exitX=0.5;exitY=1;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7234&quot; y=&quot;-20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2560&quot; y=&quot;853.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2320&quot; y=&quot;853.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-167&quot; value=&quot;&amp;lt;b style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 16px;&amp;quot;&amp;gt;lights/channel_2_frame&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=16;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=left;exitX=0.75;exitY=1;exitDx=0;exitDy=0;entryX=0.75;entryY=0;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7234&quot; y=&quot;-20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2560&quot; y=&quot;883.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint y=&quot;-1&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2606.98&quot; y=&quot;655.8200000000002&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-168&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: center; font-size: 18px;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial; font-size: 18px;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights/&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial; text-align: right; font-size: 18px;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;set_animation&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=1;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=0;exitY=0.75;exitDx=0;exitDy=0;entryX=1;entryY=0.75;entryDx=0;entryDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-161&quot; target=&quot;D_R0GpuGBRVuPwS73e86-2&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.8077&quot; y=&quot;12&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2204.63&quot; y=&quot;469.2799999999999&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2208.14&quot; y=&quot;837.225&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-170&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: center;&amp;quot;&amp;gt;&amp;lt;b&amp;gt;lights/set_brightness&amp;lt;/b&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=1;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9279&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2620&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2430.0050000000006&quot; y=&quot;815&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;safety_manager&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2012&quot; y=&quot;304.72&quot; width=&quot;190&quot; height=&quot;100&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;system_monitor&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ed3b42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2093.35&quot; y=&quot;140.57&quot; width=&quot;180&quot; height=&quot;70&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-183&quot; value=&quot;&amp;lt;b&amp;gt;system_status&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0;exitY=0.5;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.7278&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2040&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1940&quot; y=&quot;120.00000000000004&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2040&quot; y=&quot;130&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2040&quot; y=&quot;40&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-184&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0.145;entryY=0.01;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0;exitY=0.5;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9108&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2040&quot; y=&quot;280.7221600000001&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2080&quot; y=&quot;130&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2040&quot; y=&quot;176&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-187&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.876;exitY=-0.012;exitDx=0;exitDy=0;align=right;exitPerimeter=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2000&quot; y=&quot;420&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1910&quot; y=&quot;735.0912500000003&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1910&quot; y=&quot;355&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-189&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0;entryY=0.25;entryDx=0;entryDy=0;exitX=0.75;exitY=0;exitDx=0;exitDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2009&quot; y=&quot;399&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1870&quot; y=&quot;730&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-190&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0;entryY=0;entryDx=0;entryDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2000&quot; y=&quot;380&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1840&quot; y=&quot;720&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1840&quot; y=&quot;305&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-191&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;exitX=0.353;exitY=0.023;exitDx=0;exitDy=0;align=right;exitPerimeter=0;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;entryX=0;entryY=1;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9453&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2009&quot; y=&quot;359&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1760.5749999999998&quot; y=&quot;735.0887499999998&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1761&quot; y=&quot;405&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-195&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=1;align=left;exitX=1;exitY=0.25;exitDx=0;exitDy=0;entryX=1;entryY=0.5;entryDx=0;entryDy=0;fontStyle=1&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.0992&quot; y=&quot;-22&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2229.24&quot; y=&quot;400&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2149.24&quot; y=&quot;658&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2266&quot; y=&quot;668&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;668&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;331&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-204&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;exitX=0.59;exitY=0.007;exitDx=0;exitDy=0;align=right;exitPerimeter=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;jumpSize=6;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-85&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-149&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.8002&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1330&quot; y=&quot;10&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1330.0100000000002&quot; y=&quot;309.09000000000015&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1332&quot; y=&quot;300&quot; /&gt;&#10;              &lt;mxPoint x=&quot;1332&quot; y=&quot;230&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-214&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#676965;startArrow=none;startFill=0;endArrow=none;horizontal=1;align=left;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;verticalAlign=middle;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;780.0300000000002&quot; y=&quot;220.26000000000002&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;10&quot; y=&quot;10&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;720&quot; y=&quot;219.68999999999997&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;780&quot; y=&quot;219.91&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;kmaXxYwjsGMcyooEdLwJ-215&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#676965;startArrow=none;startFill=0;endArrow=none;horizontal=1;align=left;endFill=0;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1&quot; y=&quot;-10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;780&quot; y=&quot;179.75&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;10&quot; y=&quot;-10&quot; as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;720&quot; y=&quot;179.84000000000003&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;D_R0GpuGBRVuPwS73e86-2&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px;&amp;quot;&amp;gt;lights_manager&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ED3B42;fontColor=#ffffff;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;2012&quot; y=&quot;420&quot; width=&quot;190&quot; height=&quot;90&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;D_R0GpuGBRVuPwS73e86-7&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=1;align=left;entryX=1;entryY=0.25;entryDx=0;entryDy=0;fontStyle=1;exitX=1;exitY=0.25;exitDx=0;exitDy=0;jumpStyle=none;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; target=&quot;D_R0GpuGBRVuPwS73e86-2&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;-0.0992&quot; y=&quot;-22&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2340&quot; y=&quot;389.24&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2300&quot; y=&quot;700&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;668&quot; /&gt;&#10;              &lt;mxPoint x=&quot;2310&quot; y=&quot;443&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;D_R0GpuGBRVuPwS73e86-9&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;entryX=0;entryY=0.5;entryDx=0;entryDy=0;exitX=0.876;exitY=0.004;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;D_R0GpuGBRVuPwS73e86-2&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9457&quot; y=&quot;20&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;3091&quot; y=&quot;289.22&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2910&quot; y=&quot;695.22&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-3&quot; value=&quot;&amp;lt;span style=&amp;quot;forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;forced-color-adjust: none;&amp;quot;&amp;gt;&amp;lt;font style=&amp;quot;forced-color-adjust: none; font-size: 16px;&amp;quot;&amp;gt;hardware/&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;span style=&amp;quot;forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; font-size: 16px;&amp;quot;&amp;gt;&amp;lt;b style=&amp;quot;forced-color-adjust: none;&amp;quot;&amp;gt;led_control_enable&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=1;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;verticalAlign=middle;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9141&quot; y=&quot;-14&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2214.63&quot; y=&quot;1267.0600000000002&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2210&quot; y=&quot;898.3400000000001&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-6&quot; value=&quot;&amp;lt;b&amp;gt;/tf&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.389;exitY=0.011;exitDx=0;exitDy=0;exitPerimeter=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-86&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.857&quot; y=&quot;17&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1140&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1144.23&quot; y=&quot;190.72&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-8&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;battery/&amp;lt;/b&amp;gt;&amp;lt;b&amp;gt;charging_status&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=1;exitY=0.5;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9189&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2340&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2306&quot; y=&quot;678&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-14&quot; value=&quot;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=2;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=classic;startFill=1;endArrow=none;horizontal=0;align=right;dashed=1;endSize=12;startSize=8;spacing=3;endFill=0;exitX=0.071;exitY=0.012;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0;entryY=0.75;entryDx=0;entryDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-173&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9174&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1690&quot; y=&quot;10&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1620&quot; y=&quot;520&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot;&gt;&#10;              &lt;mxPoint x=&quot;1680&quot; y=&quot;380&quot; /&gt;&#10;            &lt;/Array&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-15&quot; value=&quot;&amp;lt;font style=&amp;quot;font-size: 18px; font-weight: normal;&amp;quot;&amp;gt;GPIODriver&amp;lt;/font&amp;gt;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;1650&quot; y=&quot;840.72&quot; width=&quot;130&quot; height=&quot;50&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;NPlMt0osOJUE4kA7_BK4-17&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=3;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=block;entryX=1;entryY=0.75;entryDx=0;entryDy=0;startWidth=5.555555555555555;endWidth=5.555555555555555;exitX=0;exitY=0.75;exitDx=0;exitDy=0;edgeStyle=orthogonalEdgeStyle;endFill=1;&quot; parent=&quot;1&quot; source=&quot;NPlMt0osOJUE4kA7_BK4-15&quot; target=&quot;kmaXxYwjsGMcyooEdLwJ-27&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1670&quot; y=&quot;797&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1587&quot; y=&quot;863&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-2&quot; value=&quot;&amp;lt;b&amp;gt;diagnostics&amp;lt;/b&amp;gt;&quot; style=&quot;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.75;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-118&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6747&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1093&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1070&quot; y=&quot;186&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-3&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: left;&amp;quot;&amp;gt;&amp;lt;span style=&amp;quot;background-color: initial;&amp;quot;&amp;gt;&amp;lt;b&amp;gt;diagnostics&amp;lt;/b&amp;gt;&amp;lt;/span&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=1;exitY=0.75;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-75&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9189&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2370&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2257&quot; y=&quot;700&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-4&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;diagnostics&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-174&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.6688&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2070&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2090&quot; y=&quot;186&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-6&quot; value=&quot;&amp;lt;div style=&amp;quot;text-align: left;&amp;quot;&amp;gt;&amp;lt;br&amp;gt;&amp;lt;/div&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0;exitY=0.25;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-160&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9189&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;2370&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;2257&quot; y=&quot;722&quot; as=&quot;sourcePoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-7&quot; value=&quot;&amp;lt;b style=&amp;quot;text-align: left;&amp;quot;&amp;gt;diagnostics&amp;lt;/b&amp;gt;&quot; style=&quot;edgeStyle=orthogonalEdgeStyle;shape=connector;rounded=1;orthogonalLoop=1;jettySize=auto;html=1;labelBackgroundColor=none;strokeColor=#888a85;strokeWidth=4;fontFamily=Helvetica;fontSize=18;fontColor=#888a85;startArrow=none;startFill=0;endArrow=classic;horizontal=0;align=right;exitX=0.5;exitY=0;exitDx=0;exitDy=0;&quot; parent=&quot;1&quot; source=&quot;kmaXxYwjsGMcyooEdLwJ-26&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;0.9173&quot; y=&quot;10&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;1802&quot; as=&quot;targetPoint&quot; /&gt;&#10;            &lt;mxPoint as=&quot;offset&quot; /&gt;&#10;            &lt;mxPoint x=&quot;1840&quot; y=&quot;731&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;Array as=&quot;points&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-8&quot; value=&quot;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=none;fillColor=#ED3B42;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;720&quot; y=&quot;45&quot; width=&quot;60&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-11&quot; value=&quot;&amp;lt;span style=&amp;quot;font-size: 20px; font-weight: 700; text-wrap: wrap;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot;&amp;gt;Legend:&amp;lt;/font&amp;gt;&amp;lt;/span&amp;gt;&quot; style=&quot;text;html=1;align=center;verticalAlign=middle;resizable=0;points=[];autosize=1;strokeColor=none;fillColor=none;fontSize=16;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;700&quot; y=&quot;-4.85&quot; width=&quot;100&quot; height=&quot;40&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-12&quot; value=&quot;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=#E6E6E6;fillColor=#CCCCCC;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=6;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;720&quot; y=&quot;85&quot; width=&quot;60&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-14&quot; value=&quot;&quot; style=&quot;rounded=1;whiteSpace=wrap;html=1;strokeColor=#B0B3AC;fillColor=#FFFFFF;fontColor=#000000;fontSize=19;fontFamily=Helvetica;spacing=2;spacingTop=4;fontStyle=1;strokeWidth=2;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;720&quot; y=&quot;125&quot; width=&quot;60&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-18&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Nodes&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;45&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-19&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Plugin&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;85&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-20&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Objects&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;125&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-21&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Topics&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;165&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;5hbYxZ5cZ9pdv8yiFV4I-22&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Services&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;205&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;Y5d-tTOP6FVwuIk47fc7-3&quot; value=&quot;&quot; style=&quot;endArrow=block;html=1;rounded=1;strokeColor=#FF7B80;strokeWidth=6;fontFamily=Helvetica;fontSize=20;fontColor=#ffffff;labelBackgroundColor=default;fillColor=#FFFFFF;startArrow=none;startWidth=5.555555555555555;endWidth=5.555555555555555;startFill=0;endFill=1;&quot; parent=&quot;1&quot; edge=&quot;1&quot;&gt;&#10;          &lt;mxGeometry width=&quot;50&quot; height=&quot;50&quot; relative=&quot;1&quot; as=&quot;geometry&quot;&gt;&#10;            &lt;mxPoint x=&quot;720&quot; y=&quot;260&quot; as=&quot;sourcePoint&quot; /&gt;&#10;            &lt;mxPoint x=&quot;780&quot; y=&quot;259.57&quot; as=&quot;targetPoint&quot; /&gt;&#10;          &lt;/mxGeometry&gt;&#10;        &lt;/mxCell&gt;&#10;        &lt;mxCell id=&quot;Y5d-tTOP6FVwuIk47fc7-4&quot; value=&quot;&amp;lt;b style=&amp;quot;forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;&amp;quot;&amp;gt;&amp;lt;font color=&amp;quot;#888a85&amp;quot; style=&amp;quot;forced-color-adjust: none; font-size: 18px;&amp;quot;&amp;gt;Data Transfer&amp;lt;/font&amp;gt;&amp;lt;/b&amp;gt;&quot; style=&quot;text;whiteSpace=wrap;html=1;fontSize=18;&quot; parent=&quot;1&quot; vertex=&quot;1&quot;&gt;&#10;          &lt;mxGeometry x=&quot;795&quot; y=&quot;245&quot; width=&quot;90&quot; height=&quot;30&quot; as=&quot;geometry&quot; /&gt;&#10;        &lt;/mxCell&gt;&#10;      &lt;/root&gt;&#10;    &lt;/mxGraphModel&gt;&#10;  &lt;/diagram&gt;&#10;&lt;/mxfile&gt;&#10;" resource="https://app.diagrams.net/#G1LlhIvfnfv5NcG4CBD7yvBf7X81yUhw3e"><defs/><g><g data-cell-id="0"><g data-cell-id="1"><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-23"><g><rect x="270" y="175.15" width="180" height="100" rx="6" ry="6" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 272px; margin-left: 271px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">husarion_ugv_localization</font></span></div></div></div></foreignObject><text x="360" y="272" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">husarion_ugv_localization</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-179"><g><rect x="140" y="330" width="600" height="240" rx="14.4" ry="14.4" fill-opacity="0.9" fill="#ffffff" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 598px; height: 1px; padding-top: 567px; margin-left: 141px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">husarion_ugv_controller</font></span></div></div></div></foreignObject><text x="440" y="567" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">husarion_ugv_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-199"><g><rect x="1700" y="390" width="200" height="450" rx="30" ry="30" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 397px; margin-left: 1701px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;"><font color="#1a1a1a">husarion_ugv_lights</font></span></div></div></div></foreignObject><text x="1800" y="409" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">husarion_ugv_lights</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-202"><g><rect x="140" y="600" width="1150" height="360.12" rx="21.61" ry="21.61" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="5" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 1148px; height: 1px; padding-top: 957px; margin-left: 141px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">husarion_ugv_hardware_interfaces</font></span></div></div></div></foreignObject><text x="715" y="957" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">husarion_ugv_hardware_interfaces</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-206"><g><rect x="1327.36" y="620" width="256" height="140" rx="21" ry="21" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 254px; height: 1px; padding-top: 627px; margin-left: 1328px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-size: 18px;"><font color="#1a1a1a">husarion_ugv_battery</font></span></div></div></div></foreignObject><text x="1455" y="639" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">husarion_ugv_battery</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-208"><g><rect x="1278.87" y="280" width="243" height="277.55" rx="36.45" ry="36.45" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-end; justify-content: unsafe center; width: 241px; height: 1px; padding-top: 555px; margin-left: 1280px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;"><font color="#1a1a1a">husarion_ugv_manager</font></span></div></div></div></foreignObject><text x="1400" y="555" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">husarion_ugv_manager</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-211"><g><rect x="1383.36" y="120" width="200" height="120" rx="18" ry="18" fill-opacity="0.9" fill="rgb(255, 255, 255)" stroke="#e6e6e6" stroke-opacity="0.9" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 127px; margin-left: 1384px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><span style="font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: normal; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;"><font color="#1a1a1a">husarion_ugv_diagnostics</font></span></div></div></div></foreignObject><text x="1483" y="139" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="12px" text-anchor="middle">husarion_ugv_diagnostics</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-24"><g><rect x="455" y="620.72" width="815" height="311.5" rx="18.69" ry="18.69" fill="#cccccc" stroke="#e6e6e6" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 813px; height: 1px; padding-top: 632px; margin-left: 456px;"><div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 24px;" color="#1a1a1a">{Robot}System</font></div></div></div></foreignObject><text x="863" y="651" fill="#FFFFFF" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">{Robot}System</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-26"><g><rect x="960" y="730.72" width="285" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 283px; height: 1px; padding-top: 778px; margin-left: 961px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><span style="color: rgb(255, 255, 255); text-align: left;"><font style="font-size: 18px;">hardware_controller</font></span></div></div></div></foreignObject><text x="1103" y="783" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">hardware_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-27"><g><rect x="716.88" y="850.72" width="160" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 878px; margin-left: 718px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">GPIOController</font></div></div></div></foreignObject><text x="797" y="883" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">GPIOController</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-28"><g><rect x="645" y="680.72" width="190" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 708px; margin-left: 646px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">MotorsController</font></div></div></div></foreignObject><text x="740" y="713" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">MotorsController</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-29"><g><rect x="476" y="850.72" width="200" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 878px; margin-left: 477px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">CANopenController</font></div></div></div></foreignObject><text x="576" y="883" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">CANopenController</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-30"><g><rect x="501" y="765.72" width="150" height="47.5" rx="7.13" ry="7.13" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 148px; height: 1px; padding-top: 791px; margin-left: 502px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">RoboteqDriver</font></div></div></div></foreignObject><text x="576" y="797" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">RoboteqDriver</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-31"><g><rect x="507.25" y="980.72" width="137.5" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 136px; height: 1px; padding-top: 1011px; margin-left: 508px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">CAN</div></div></div></foreignObject><text x="576" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">CAN</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-32"><g><path d="M 576 919.43 L 576 962.01" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 576 907.43 L 582 919.43 L 570 919.43 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 576 974.01 L 570 962.01 L 582 962.01 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-35"><g><path d="M 1015 919.43 L 1015 962.01" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1015 907.43 L 1021 919.43 L 1009 919.43 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 1015 974.01 L 1009 962.01 L 1021 962.01 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-38"><g><rect x="715" y="763.22" width="160" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 790px; margin-left: 716px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">EStopManager</font></div></div></div></foreignObject><text x="795" y="796" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">EStopManager</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-56"><g><path d="M 796.78 743.27 L 796.78 750.87" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 796.78 734.27 L 801.28 743.27 L 792.28 743.27 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 796.78 759.87 L 792.28 750.87 L 801.28 750.87 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-58"><g><path d="M 796.79 825.57 L 796.83 838.37" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 796.75 816.57 L 801.29 825.56 L 792.29 825.59 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 796.87 847.37 L 792.33 838.38 L 801.33 838.35 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-63"><g><path d="M 632.65 705.72 L 586 705.7 Q 576 705.7 576 715.7 L 576 753.37" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 641.65 705.72 L 632.64 710.22 L 632.65 701.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 576 762.37 L 571.5 753.37 L 580.5 753.37 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-64"><g><path d="M 576 825.57 L 576 838.37" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 576 816.57 L 580.5 825.57 L 571.5 825.57 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 576 847.37 L 571.5 838.37 L 580.5 838.37 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-66"><g><path d="M 947.65 775.72 L 887.35 775.72" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 956.65 775.72 L 947.65 780.22 L 947.65 771.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 878.35 775.72 L 887.35 771.22 L 887.35 780.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-73"><g><path d="M 947.65 753.22 L 907.5 753.2 Q 897.5 753.2 897.5 743.2 L 897.5 715.7 Q 897.5 705.7 887.5 705.7 L 847.35 705.72" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 956.65 753.22 L 947.64 757.72 L 947.65 748.72 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 838.35 705.72 L 847.35 701.22 L 847.36 710.22 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-74"><g><path d="M 947.65 798.21 L 928.5 798.2 Q 918.5 798.2 918.5 808.2 L 918.5 853.2 Q 918.5 863.2 908.5 863.2 L 889.23 863.21" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 956.65 798.22 L 947.64 802.71 L 947.65 793.71 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 880.23 863.22 L 889.23 858.71 L 889.24 867.71 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-75"><g><rect x="1357.36" y="654.84" width="190" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 702px; margin-left: 1358px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">battery_driver</font></div></div></div></foreignObject><text x="1452" y="708" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">battery_driver</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-77"><g><path d="M 1452.36 980.72 L 1452.36 763.55" fill="none" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1452.36 751.55 L 1458.36 763.55 L 1446.36 763.55 Z" fill="#888a85" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-79"><g><rect x="950" y="980.72" width="130" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 1011px; margin-left: 951px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">RPi GPIO</div></div></div></foreignObject><text x="1015" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">RPi GPIO</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-80"><g><rect x="1383.61" y="980.72" width="137.5" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 136px; height: 1px; padding-top: 1011px; margin-left: 1385px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">ADC</div></div></div></foreignObject><text x="1452" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">ADC</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-84"><g><rect x="195" y="484.72" width="180" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 517px; margin-left: 196px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">imu_broadcaster</font></div></div></div></foreignObject><text x="285" y="522" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">imu_broadcaster</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-85"><g><rect x="490" y="347" width="240" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 379px; margin-left: 491px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">joint_state_broadcaster</font></div></div></div></foreignObject><text x="610" y="385" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">joint_state_broadcaster</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-86"><g><rect x="370" y="424.72" width="180" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 457px; margin-left: 371px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">drive_controller</font></div></div></div></foreignObject><text x="460" y="462" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">drive_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-95"><g><rect x="32.5" y="422.5" width="220" height="55" rx="8.25" ry="8.25" fill="#ed3b42" stroke="none" transform="rotate(270,142.5,450)" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 144.5 450)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 218px; height: 1px; padding-top: 450px; margin-left: 36px;"><div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><div style="text-align: left;"><span style="background-color: initial;"><font style="font-size: 18px;">controller_manager</font></span></div></div></div></div></foreignObject><text x="145" y="456" fill="#FFFFFF" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">controller_manager</text></switch></g></g></g><g data-cell-id="daj83a5Ns2Ukew12XzJU-1"><g><path d="M 285 620.72 L 285 551.09" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 285 545.84 L 288.5 552.84 L 285 551.09 L 281.5 552.84 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="daj83a5Ns2Ukew12XzJU-2"><g><path d="M 285 620.72 L 285 551.09" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 285 545.84 L 288.5 552.84 L 285 551.09 L 281.5 552.84 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-99"><g><rect x="160" y="620.72" width="250" height="311.5" rx="15" ry="15" fill="#cccccc" stroke="#e6e6e6" stroke-width="6" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 248px; height: 1px; padding-top: 632px; margin-left: 161px;"><div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><div style="font-size: 24px;"><font style="font-size: 24px;" color="#1a1a1a">PhidgetImuSensor</font></div><div style="font-size: 24px;"><br /></div></div></div></div></foreignObject><text x="285" y="651" fill="#FFFFFF" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">PhidgetImuSensor&#xa;</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-103"><g><rect x="216.25" y="980.72" width="137.5" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 136px; height: 1px; padding-top: 1011px; margin-left: 217px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 20px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;">IMU</div></div></div></foreignObject><text x="285" y="1017" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="20px" text-anchor="middle" font-weight="bold">IMU</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-106"><g><path d="M 285 563.43 L 285 602.01" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 285 551.43 L 291 563.43 L 279 563.43 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 285 614.01 L 279 602.01 L 291 602.01 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-116"><g><path d="M 655.22 424.02 L 658.41 602.02" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 655 412.03 L 661.22 423.92 L 649.22 424.13 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 658.63 614.01 L 652.41 602.12 L 664.41 601.91 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-117"><g><path d="M 530.24 504.93 L 531.39 604.51" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 530.1 492.93 L 536.24 504.86 L 524.24 505 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/><path d="M 531.53 616.5 L 525.39 604.57 L 537.39 604.44 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-118"><g><rect x="295" y="185.72" width="130" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 218px; margin-left: 296px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">ekf_filter</font></div></div></div></foreignObject><text x="360" y="223" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">ekf_filter</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-120"><g><rect x="180" y="705.22" width="210" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 208px; height: 1px; padding-top: 732px; margin-left: 181px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">ImuFilter</font></div></div></div></foreignObject><text x="285" y="738" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">ImuFilter</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-121"><g><rect x="190" y="831.34" width="190" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 858px; margin-left: 191px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">Spatial</font></div></div></div></foreignObject><text x="285" y="864" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">Spatial</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-123"><g><path d="M 284.5 980.22 L 284.91 900.05" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 284.97 888.05 L 290.91 900.08 L 278.91 900.02 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-124"><g><path d="M 284.66 831.34 L 284.92 773.93" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 284.97 761.93 L 290.92 773.95 L 278.92 773.9 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-125"><g><path d="M 240 484.72 L 240 257.9 Q 240 247.9 240 237.9 L 240 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 240 14.47 L 245 24.47 L 240 21.97 L 235 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 223.05934473723292 38.25198888361385)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 38px; margin-left: 223px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>imu/data</b></div></div></div></foreignObject><text x="223" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">imu/data</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-129"><g><path d="M 392.32 425.08 L 392.49 257.69" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 392.5 250.19 L 397.49 260.2 L 392.49 257.69 L 387.49 260.19 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 379.32602896003704 418.0649495053864)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 1px; height: 1px; padding-top: 418px; margin-left: 379px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b><font style="font-size: 18px;">odometry/wheels</font></b></div></div></div></foreignObject><text x="379" y="423" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px">odometry/wheels</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-130"><g><path d="M 327.5 185.72 L 327.97 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 327.99 14.47 L 332.96 24.49 L 327.97 21.97 L 322.96 24.46 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 315.918317869348 38.69214084552837)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 316px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>odometry/filtered</b></div></div></div></foreignObject><text x="316" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">odometry/filtered</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-131"><g><path d="M 240 484.72 L 240 225.7 Q 240 215.7 250 215.7 L 283.03 215.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 290.53 215.72 L 280.53 220.71 L 283.03 215.72 L 280.53 210.71 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-134"><g><path d="M 631.12 347.78 L 631.1 189 Q 631.1 179 631.03 169 L 630.08 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 630.03 14.47 L 635.09 24.44 L 630.08 21.97 L 625.09 24.5 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 618.1017614886418 44.4101760279836)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 44px; margin-left: 618px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>joint_states</b></div></div></div></foreignObject><text x="618" y="50" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">joint_states</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-135"><g><path d="M 470 10 L 470.48 207.9 Q 470.5 217.9 470.5 227.9 L 470.44 418.03" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 470.44 425.53 L 465.44 415.53 L 470.44 418.03 L 475.44 415.53 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 453.0721932435777 40.03598817157367)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 453px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>cmd_vel</b></div></div></div></foreignObject><text x="453" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">cmd_vel</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-136"><g><path d="M 582.16 347.12 L 582.2 189 Q 582.2 179 582.13 169 L 581.09 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 581.03 14.47 L 586.1 24.44 L 581.09 21.97 L 576.1 24.51 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 569.0971461347985 42.85018642894648)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 43px; margin-left: 569px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>dynamic_joint_states</b></div></div></div></foreignObject><text x="569" y="48" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">dynamic_joint_states</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-138"><g><path d="M 1209.66 730.54 L 1209.7 380.9 Q 1209.7 370.9 1209.71 360.9 L 1209.99 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1210 14.47 L 1214.99 24.48 L 1209.99 21.97 L 1204.99 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1198.040658419977 39.35417466263085)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 1198px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;"> hardware/e_stop</b></div></div></div></foreignObject><text x="1198" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end"> hardware/e_stop</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-140"><g><path d="M 1139.55 732.43 L 1139.6 380.9 Q 1139.6 370.9 1139.61 360.9 L 1139.99 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1140 14.47 L 1144.98 24.48 L 1139.99 21.97 L 1134.98 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1128.0420299914413 40.34417494560757)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1128px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>hardware/robot_driver_state</b></div></div></div></foreignObject><text x="1128" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/robot_driver_state</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-143"><g><path d="M 1173.75 730.72 L 1173.8 380.9 Q 1173.8 370.9 1173.81 360.9 L 1173.99 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1174 14.47 L 1178.99 24.47 L 1173.99 21.97 L 1168.99 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1162.0407692556278 39.434175835366155)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 1162px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;"> hardware/io_state</b></div></div></div></foreignObject><text x="1162" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end"> hardware/io_state</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-144"><g><path d="M 980.23 721.07 L 980.2 380.9 Q 980.2 370.9 980.19 360.9 L 980 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 980.23 728.57 L 975.23 718.57 L 980.23 721.07 L 985.23 718.57 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 967.0417251957298 40.1241734010298)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 967px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;">hardware/</b><b>e_stop_trigger</b></div></div></div></foreignObject><text x="967" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/e_stop_trigger</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-145"><g><path d="M 1020.7 720.62 L 1020.7 380.9 Q 1020.7 370.9 1020.68 360.9 L 1020 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1020.7 728.12 L 1015.7 718.12 L 1020.7 720.62 L 1025.7 718.12 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1007.0409632102225 39.57417226182657)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1007px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;">hardware/</b><b>e_stop_reset</b></div></div></div></foreignObject><text x="1007" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/e_stop_reset</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-148"><g><path d="M 1060.6 723.59 L 1060.6 380.9 Q 1060.6 370.9 1060.58 360.9 L 1060 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1060.6 731.09 L 1055.6 721.09 L 1060.6 723.59 L 1065.6 721.09 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1047.0407831051025 39.444172386302576)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 1047px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: center;">hardware/</b><b><i>&lt;hw_component&gt;</i>_enable</b></div></div></div></foreignObject><text x="1047" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/&lt;hw_component&gt;_enable</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-149"><g><rect x="665" y="210" width="240" height="60" rx="9" ry="9" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 242px; margin-left: 666px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">robot_state_publisher</font></div></div></div></foreignObject><text x="785" y="248" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">robot_state_publisher</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-150"><g><path d="M 830.01 210 L 830 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 830 14.47 L 835 24.47 L 830 21.97 L 825 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 818.0015500125105 41.00050021124936)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 41px; margin-left: 818px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>robot_description</b></div></div></div></foreignObject><text x="818" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">robot_description</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-151"><g><path d="M 740 210 L 740 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 740 14.47 L 745 24.47 L 740 21.97 L 735 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 728 41)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 41px; margin-left: 728px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf</b></div></div></div></foreignObject><text x="728" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-152"><g><path d="M 785 210 L 785 120.5 Q 785 110.5 785 100.5 L 785 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 785 14.47 L 790 24.47 L 785 21.97 L 780 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 771.1618442375116 41.560542301394726)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 42px; margin-left: 771px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf_static</b></div></div></div></foreignObject><text x="771" y="47" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf_static</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-153"><g><path d="M 360 185.72 L 360 108.4 Q 360 98.4 360 88.4 L 360 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 360 14.47 L 365 24.47 L 360 21.97 L 355 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 348.1597807831874 38.27752278433415)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 38px; margin-left: 348px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf</b></div></div></div></foreignObject><text x="348" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-156"><g><path d="M 1547.36 677.34 L 1600.5 677.31 Q 1610.5 677.3 1610.49 667.3 L 1610.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1610 14.47 L 1615.01 24.47 L 1610.01 21.97 L 1605.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1598.0228112826962 40.44768432687242)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1598px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">battery/battery_status</b></div></div></div></foreignObject><text x="1598" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">battery/battery_status</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-158"><g><path d="M 1173.75 730.72 L 1173.79 690 Q 1173.8 680 1183.8 680 L 1345.39 680" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1352.89 680 L 1342.89 685 L 1345.39 680 L 1342.89 675 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-159"><g><rect x="1720" y="980.72" width="160" height="60" rx="9" ry="9" fill="#434343" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 1011px; margin-left: 1721px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><div style="font-size: 20px;"><span style="">Bumper Lights</span><br /></div></div></div></div></foreignObject><text x="1800" y="1016" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="16px" text-anchor="middle" font-weight="bold">Bumper Lights&#xa;</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-160"><g><rect x="1710" y="731.47" width="180" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 778px; margin-left: 1711px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">lights_driver</font></div></div></div></foreignObject><text x="1800" y="784" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">lights_driver</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-161"><g><rect x="1710" y="430" width="180" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 477px; margin-left: 1711px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">lights_controller</font></div></div></div></foreignObject><text x="1800" y="483" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">lights_controller</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-162"><g><path d="M 1800 821.47 L 1800 962.01" fill="none" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1800 974.01 L 1794 962.01 L 1806 962.01 Z" fill="#888a85" stroke="#888a85" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-165"><g><path d="M 1800 520 L 1800 719.5" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1800 727 L 1795 717 L 1800 719.5 L 1805 717 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1782 701)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 1px; height: 1px; padding-top: 701px; margin-left: 1782px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="font-size: 16px;"><font style="font-size: 16px;">lights/channel_1_frame</font></b></div></div></div></foreignObject><text x="1782" y="706" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="16px">lights/channel_1_frame</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-167"><g><path d="M 1845 520 L 1845 719.5" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1845 727 L 1840 717 L 1845 719.5 L 1850 717 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1827 701)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 1px; height: 1px; padding-top: 701px; margin-left: 1827px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="font-size: 16px;"><font style="font-size: 16px;">lights/channel_2_frame</font></b></div></div></div></foreignObject><text x="1827" y="706" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="16px">lights/channel_2_frame</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-168"><g><path d="M 1700.26 497.5 L 1502 497.5" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1707.76 497.5 L 1697.76 502.5 L 1700.26 497.5 L 1697.76 492.5 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 510px; margin-left: 1687px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: center; font-size: 18px;"><span style="background-color: initial; font-size: 18px;"><b style="font-size: 18px;"><font style="font-size: 18px;">lights/</font></b></span><span style="background-color: initial; text-align: right; font-size: 18px;"><b style="font-size: 18px;">set_animation</b></span></div></div></div></div></foreignObject><text x="1687" y="515" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">lights/set_animation</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-170"><g><path d="M 1899.74 753.98 L 1910.5 753.99 Q 1920.5 754 1920.49 744 L 1920 10" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1892.24 753.97 L 1902.24 748.98 L 1899.74 753.98 L 1902.23 758.98 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1907.0184835596563 37.50689698330439)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 38px; margin-left: 1907px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: center;"><b>lights/set_brightness</b><br /></div></div></div></div></foreignObject><text x="1907" y="43" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">lights/set_brightness&#xa;</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-173"><g><rect x="1312" y="314.72" width="190" height="100" rx="15" ry="15" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 367px; margin-left: 1313px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">safety_manager</font></div></div></div></foreignObject><text x="1407" y="372" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">safety_manager</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-174"><g><rect x="1393.35" y="150.57" width="180" height="70" rx="10.5" ry="10.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 188px; margin-left: 1394px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">system_monitor</font></div></div></div></foreignObject><text x="1483" y="193" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">system_monitor</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-183"><g><path d="M 1393.35 185.57 L 1350 185.59 Q 1340 185.6 1340 175.6 L 1340 150 Q 1340 140 1340 130 L 1340 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1340 14.47 L 1345 24.47 L 1340 21.97 L 1335 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1328 40.95000843486335)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 41px; margin-left: 1328px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>system_status</b></div></div></div></foreignObject><text x="1328" y="46" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">system_status</text></switch></g></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-184"><g><path d="M 1393.35 185.57 L 1349.6 185.59 Q 1339.6 185.6 1339.6 195.6 L 1339.55 303.75" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1339.55 311.25 L 1334.56 301.25 L 1339.55 303.75 L 1344.56 301.25 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-187"><g><path d="M 1209.66 729.64 L 1209.7 374.7 Q 1209.7 364.7 1219.7 364.7 L 1300.03 364.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 364.72 L 1297.53 369.72 L 1300.03 364.72 L 1297.53 359.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-189"><g><path d="M 1173.75 730.72 L 1173.8 349.7 Q 1173.8 339.7 1183.8 339.7 L 1300.03 339.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 339.72 L 1297.53 344.72 L 1300.03 339.72 L 1297.53 334.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-190"><g><path d="M 1140 730 L 1140 324.7 Q 1140 314.7 1150 314.7 L 1300.03 314.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 314.72 L 1297.53 319.72 L 1300.03 314.72 L 1297.53 309.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-191"><g><path d="M 1060.6 723.05 L 1060.6 424.7 Q 1060.6 414.7 1070.6 414.7 L 1312 414.72" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1060.6 730.55 L 1055.6 720.55 L 1060.6 723.05 L 1065.6 720.55 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-195"><g><path d="M 1547.36 677.34 L 1556.68 677.32 Q 1566 677.3 1576 677.46 L 1600 677.84 Q 1610 678 1610 668 L 1610 374.7 Q 1610 364.7 1600 364.7 L 1513.97 364.72" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1506.47 364.72 L 1516.47 359.72 L 1513.97 364.72 L 1516.47 369.72 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-204"><g><path d="M 631.6 347.42 L 631.6 320 Q 631.6 310 631.66 300 L 631.94 250 Q 632 240 642 240 L 653.03 240" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 660.53 240 L 650.53 245 L 653.03 240 L 650.53 235 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-214"><g><path d="M 20 230.3 L 80.03 230.26" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/></g></g><g data-cell-id="kmaXxYwjsGMcyooEdLwJ-215"><g><path d="M 20 189.84 L 80 189.75" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/></g></g><g data-cell-id="D_R0GpuGBRVuPwS73e86-2"><g><rect x="1312" y="430" width="190" height="90" rx="13.5" ry="13.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 188px; height: 1px; padding-top: 477px; margin-left: 1313px;"><div data-drawio-colors="color: #ffffff; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px;">lights_manager</font></div></div></div></foreignObject><text x="1407" y="483" fill="#ffffff" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">lights_manager</text></switch></g></g></g><g data-cell-id="D_R0GpuGBRVuPwS73e86-7"><g><path d="M 1547.36 677.34 L 1600 677.31 Q 1610 677.3 1610 667.3 L 1610 462.5 Q 1610 452.5 1600 452.5 L 1513.97 452.5" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1506.47 452.5 L 1516.47 447.5 L 1513.97 452.5 L 1516.47 457.5 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="D_R0GpuGBRVuPwS73e86-9"><g><path d="M 1209.66 731.08 L 1209.7 485 Q 1209.7 475 1219.7 475 L 1300.03 475" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1307.53 475 L 1297.53 480 L 1300.03 475 L 1297.53 470 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-3"><g><path d="M 1254.74 775.72 L 1467.5 775.7 Q 1477.5 775.7 1487.5 775.73 L 1710 776.47" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 1247.24 775.72 L 1257.24 770.72 L 1254.74 775.72 L 1257.24 780.72 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 790px; margin-left: 1687px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><span style="forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><b style="forced-color-adjust: none;"><font style="forced-color-adjust: none; font-size: 16px;">hardware/</font></b></span><span style="forced-color-adjust: none; color: rgb(136, 138, 133); font-family: Helvetica; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: right; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; background-color: initial; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; font-size: 16px;"><b style="forced-color-adjust: none;">led_control_enable</b></span></div></div></div></foreignObject><text x="1687" y="796" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">hardware/led_control_enable</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-6"><g><path d="M 440.02 425.38 L 440 227.9 Q 440 217.9 440 207.9 L 440 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 440 14.47 L 445 24.47 L 440 21.97 L 435 24.47 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 421.0719118764564 39.92140072145239)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 421px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>/tf</b></div></div></div></foreignObject><text x="421" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">/tf</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-8"><g><path d="M 1547.36 699.84 L 1630.5 699.8 Q 1640.5 699.8 1640.49 689.8 L 1640.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1640 14.47 L 1645.01 24.47 L 1640.01 21.97 L 1635.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1628.0231543966713 41.947429886303745)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 42px; margin-left: 1628px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">battery/</b><b>charging_status</b></div></div></div></foreignObject><text x="1628" y="47" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">battery/charging_status</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-14"><g><path d="M 980.23 722.06 L 980.2 399.7 Q 980.2 389.7 990.2 389.7 L 1312 389.72" fill="none" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 980.23 729.56 L 975.23 719.56 L 980.23 722.06 L 985.23 719.56 Z" fill="#888a85" stroke="#888a85" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-15"><g><rect x="950" y="850.72" width="130" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 878px; margin-left: 951px;"><div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 19px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"><font style="font-size: 18px; font-weight: normal;">GPIODriver</font></div></div></div></foreignObject><text x="1015" y="883" fill="#000000" font-family="&quot;Helvetica&quot;" font-size="19px" text-anchor="middle" font-weight="bold">GPIODriver</text></switch></g></g></g><g data-cell-id="NPlMt0osOJUE4kA7_BK4-17"><g><path d="M 937.65 888.22 L 889.23 888.22" fill="none" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 946.65 888.22 L 937.65 892.72 L 937.65 883.72 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><path d="M 880.23 888.22 L 889.23 883.72 L 889.23 892.72 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-2"><g><path d="M 392.5 185.72 L 392.97 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 392.99 14.47 L 397.96 24.49 L 392.97 21.97 L 387.96 24.46 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 380.918317869348 38.69214084552837)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 39px; margin-left: 381px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b>diagnostics</b></div></div></div></foreignObject><text x="381" y="44" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-3"><g><path d="M 1547.36 722.34 L 1660.5 722.3 Q 1670.5 722.3 1670.49 712.3 L 1670.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1670 14.47 L 1675.01 24.47 L 1670.01 21.97 L 1665.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1658.02347584129 43.447193258440564)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 43px; margin-left: 1658px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: left;"><span style="background-color: initial;"><b>diagnostics</b></span></div></div></div></div></foreignObject><text x="1658" y="49" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-4"><g><path d="M 1393.35 168.07 L 1380.5 168.09 Q 1370.5 168.1 1370.47 158.1 L 1370.04 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1370.01 14.47 L 1375.05 24.46 L 1370.04 21.97 L 1365.05 24.49 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1358.0947706300105 39.98228594656824)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1358px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">diagnostics</b></div></div></div></foreignObject><text x="1358" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-6"><g><path d="M 1710 753.97 L 1680.5 753.99 Q 1670.5 754 1670.49 744 L 1670.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1670 14.47 L 1675.01 24.47 L 1670.01 21.97 L 1665.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1658.021171728833 41.506892718330505)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 42px; margin-left: 1658px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><div style="text-align: left;"><br /></div></div></div></div></foreignObject><text x="1658" y="47" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">&#xa;</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-7"><g><path d="M 1102.5 730.72 L 1102.01 21.97" fill="none" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 1102 14.47 L 1107.01 24.47 L 1102.01 21.97 L 1097.01 24.48 Z" fill="#888a85" stroke="#888a85" stroke-width="4" stroke-miterlimit="10" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)rotate(-90 1090.0206207924284 39.727103790911734)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-end; width: 1px; height: 1px; padding-top: 40px; margin-left: 1090px;"><div data-drawio-colors="color: #888a85; " style="box-sizing: border-box; font-size: 0px; text-align: right;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(136, 138, 133); line-height: 1.2; pointer-events: all; white-space: nowrap;"><b style="text-align: left;">diagnostics</b></div></div></div></foreignObject><text x="1090" y="45" fill="#888a85" font-family="&quot;Helvetica&quot;" font-size="18px" text-anchor="end">diagnostics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-8"><g><rect x="20" y="55" width="60" height="30" rx="4.5" ry="4.5" fill="#ed3b42" stroke="none" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-11"><g><rect x="0" y="5.15" width="100" height="40" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 25px; margin-left: 50px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: nowrap;"><span style="font-size: 20px; font-weight: 700; text-wrap: wrap;"><font color="#888a85">Legend:</font></span></div></div></div></foreignObject><text x="50" y="30" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="16px" text-anchor="middle">Legend:</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-12"><g><rect x="20" y="95" width="60" height="30" rx="4.5" ry="4.5" fill="#cccccc" stroke="#e6e6e6" stroke-width="6" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-14"><g><rect x="20" y="135" width="60" height="30" rx="4.5" ry="4.5" fill="#ffffff" stroke="#b0b3ac" stroke-width="2" pointer-events="all"/></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-18"><g><rect x="95" y="55" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 62px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Nodes</font></b></div></div></div></foreignObject><text x="97" y="80" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Nodes</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-19"><g><rect x="95" y="95" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 102px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Plugin</font></b></div></div></div></foreignObject><text x="97" y="120" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Plugin</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-20"><g><rect x="95" y="135" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 142px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Objects</font></b></div></div></div></foreignObject><text x="97" y="160" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Objects</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-21"><g><rect x="95" y="175" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 182px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Topics</font></b></div></div></div></foreignObject><text x="97" y="200" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Topics</text></switch></g></g></g><g data-cell-id="5hbYxZ5cZ9pdv8yiFV4I-22"><g><rect x="95" y="215" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 222px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Services</font></b></div></div></div></foreignObject><text x="97" y="240" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Services</text></switch></g></g></g><g data-cell-id="Y5d-tTOP6FVwuIk47fc7-3"><g><path d="M 20 270 L 61.29 269.7" fill="none" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 73.29 269.62 L 61.34 275.7 L 61.25 263.7 Z" fill="#ff7b80" stroke="#ff7b80" stroke-width="6" stroke-miterlimit="10" pointer-events="all"/></g></g><g data-cell-id="Y5d-tTOP6FVwuIk47fc7-4"><g><rect x="95" y="255" width="90" height="30" fill="none" stroke="none" pointer-events="all"/></g><g><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe flex-start; justify-content: unsafe flex-start; width: 88px; height: 1px; padding-top: 262px; margin-left: 97px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;"><div style="display: inline-block; font-size: 18px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;"><b style="forced-color-adjust: none; font-family: Helvetica; font-size: 18px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; letter-spacing: normal; orphans: 2; text-align: left; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; white-space: nowrap; text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial;"><font style="forced-color-adjust: none; font-size: 18px;" color="#888a85">Data Transfer</font></b></div></div></div></foreignObject><text x="97" y="280" fill="rgb(0, 0, 0)" font-family="&quot;Helvetica&quot;" font-size="18px">Data Trans...</text></switch></g></g></g></g></g></g><switch><g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/><a transform="translate(0,-5)" xlink:href="https://www.drawio.com/doc/faq/svg-export-text-problems" target="_blank"><text text-anchor="middle" font-size="10px" x="50%" y="100%">Text is not SVG - cannot display</text></a></switch></svg>
diff --git a/.github/workflows/release-project.yaml b/.github/workflows/release-project.yaml
index 25782ec7..4ac40ef2 100644
--- a/.github/workflows/release-project.yaml
+++ b/.github/workflows/release-project.yaml
@@ -1,5 +1,5 @@
 ---
-name: Release Panther project
+name: Release project
 
 on:
   workflow_dispatch:
diff --git a/.github/workflows/run-unit-tests.yaml b/.github/workflows/run-unit-tests.yaml
index 6d7d4878..48c1ebf6 100644
--- a/.github/workflows/run-unit-tests.yaml
+++ b/.github/workflows/run-unit-tests.yaml
@@ -1,5 +1,5 @@
 ---
-name: Run panther unit tests
+name: Run unit tests
 
 on:
   workflow_dispatch:
@@ -28,12 +28,12 @@ jobs:
         uses: actions/checkout@v3
         with:
           ref: ${{ github.ref }}
-          path: ros2_ws/src/panther_ros
+          path: ros2_ws/src/husarion_ugv
 
       - name: Resolve dependencies
         working-directory: ros2_ws
         run: |
-          vcs import < src/panther_ros/panther/panther_hardware.repos src
+          vcs import < src/husarion_ugv/husarion_ugv/hardware_deps.repos src
           sudo apt update
           rosdep update --rosdistro $ROS_DISTRO
           rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y
@@ -43,15 +43,15 @@ jobs:
         run: |
           source /opt/ros/$ROS_DISTRO/setup.bash
           if [ -f install/setup.bash ]; then source install/setup.bash; fi
-          colcon build --symlink-install --parallel-workers $(nproc) --packages-up-to panther --cmake-args -DCMAKE_CXX_FLAGS='-fprofile-arcs -ftest-coverage'
+          colcon build --symlink-install --parallel-workers $(nproc) --packages-up-to husarion_ugv --cmake-args -DCMAKE_CXX_FLAGS='-fprofile-arcs -ftest-coverage'
 
       - name: Test
         working-directory: ros2_ws
         run: |
           source install/setup.bash
-          colcon test --packages-up-to panther --retest-until-pass 10 --event-handlers console_cohesion+ --return-code-on-test-failure
+          colcon test --packages-up-to husarion_ugv --retest-until-pass 10 --event-handlers console_cohesion+ --return-code-on-test-failure
           echo "result=$?" >> ${{ runner.temp }}/${{ env.TEST_RESULT_FILENAME }}
-          colcon lcov-result --packages-up-to panther --verbose >> ${{ runner.temp }}/${{ env.COVERAGE_RESULT_FILENAME }}
+          colcon lcov-result --packages-up-to husarion_ugv --verbose >> ${{ runner.temp }}/${{ env.COVERAGE_RESULT_FILENAME }}
           lines_cov=$(cat ${{ runner.temp }}/${{ env.COVERAGE_RESULT_FILENAME }} | grep -E 'lines' | head -1)
           functions_cov=$(cat ${{ runner.temp }}/${{ env.COVERAGE_RESULT_FILENAME }}  | grep -E 'functions' | head -1)
           branches_cov=$(cat ${{ runner.temp }}/${{ env.COVERAGE_RESULT_FILENAME }}  | grep -E 'branches' | head -1)
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index c426d210..a685e4f1 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -47,7 +47,7 @@ repos:
         entry: codespell
         args:
           [
-            "--exclude-file=panther_hardware_interfaces/config/roboteq_motor_controllers_v80_21a.eds",
+            "--exclude-file=husarion_ugv_hardware_interfaces/config/roboteq_motor_controllers_v80_21a.eds",
             "--ignore-words-list",
             "ned" # north, east, down (NED)
           ]
diff --git a/README.md b/README.md
index 0ec1b290..77b854e9 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
-# panther_ros
+# husarion_ugv
 
-ROS 2 packages for Panther autonomous mobile robot
+ROS 2 packages for Husarion UGV (Unmanned Ground Vehicle). The repository is a collection of necessary packages enabling the launch of the Lynx and Panther robots.
 
 [![pre-commit](https://img.shields.io/badge/pre--commit-enabled-brightgreen?logo=pre-commit)](https://github.com/pre-commit/pre-commit)
 
@@ -16,7 +16,7 @@ ROS 2 packages for Panther autonomous mobile robot
 ```bash
 mkdir ~/husarion_ws
 cd ~/husarion_ws
-git clone -b ros2 https://github.com/husarion/panther_ros.git src/panther_ros
+git clone -b ros2 https://github.com/husarion/panther_ros.git src/husarion_ugv
 ```
 
 ### Configure environment
@@ -38,7 +38,7 @@ export HUSARION_ROS_BUILD_TYPE=simulation
 ### Build
 
 ``` bash
-vcs import src < src/panther_ros/panther/panther_$HUSARION_ROS_BUILD_TYPE.repos
+vcs import src < src/husarion_ugv/husarion_ugv/${HUSARION_ROS_BUILD_TYPE}_deps.repos
 
 cp -r src/ros2_controllers/diff_drive_controller src
 cp -r src/ros2_controllers/imu_sensor_broadcaster src
@@ -49,26 +49,26 @@ rosdep update --rosdistro $ROS_DISTRO
 rosdep install --from-paths src -y -i
 
 source /opt/ros/$ROS_DISTRO/setup.bash
-colcon build --symlink-install --packages-up-to panther --cmake-args -DCMAKE_BUILD_TYPE=Release
+colcon build --symlink-install --packages-up-to husarion_ugv --cmake-args -DCMAKE_BUILD_TYPE=Release
 
 source install/setup.bash
 ```
 
 >[!NOTE]
-> To build code on a real robot you need to run above commands on the Panther Built-in Computer.
+> To build code on a real robot you need to run above commands on the robot Built-in Computer.
 
 ### Running
 
 Real robot:
 
 ```bash
-ros2 launch panther_bringup bringup.launch.py
+ros2 launch husarion_ugv_bringup bringup.launch.py
 ```
 
 Simulation:
 
 ```bash
-ros2 launch panther_gazebo simulation.launch.py
+ros2 launch husarion_ugv_gazebo simulation.launch.py
 ```
 
 > [!IMPORTANT]
@@ -87,26 +87,26 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | --- | --- | ---------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
 | ❌   | ✅   | `add_wheel_joints`           | Flag enabling joint_state_publisher to publish information about the wheel position. Should be false when there is a controller that sends this information. <br/> ***bool:*** `False`                                                                                                                             |
 | ❌   | ✅   | `add_world_transform`        | Adds a world frame that connects the tf trees of individual robots (useful when running multiple robots). <br/> ***bool:*** `False`                                                                                                                                                                                |
-| ✅   | ✅   | `animations_config_path`     | Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. <br/> ***string:*** [`{robot_model}_animations.yaml`](./panther_lights/config)                                                                                |
+| ✅   | ✅   | `animations_config_path`     | Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. <br/> ***string:*** [`{robot_model}_animations.yaml`](./husarion_ugv_lights/config)                                                                           |
 | ❌   | ✅   | `battery_config_path`        | Path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only. <br/> ***string:*** `None`                                                                                                                                                                |
-| ✅   | ✅   | `components_config_path`     | Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options). <br/> ***string:*** [`components.yaml`](./panther_description/config/components.yaml) |
-| ✅   | ✅   | `controller_config_path`     | Path to controller configuration file. A path to custom configuration can be specified here. <br/> ***string:*** [`{wheel_type}_controller.yaml`](./panther_controller/config/)                                                                                                                                    |
+| ✅   | ✅   | `components_config_path`     | Additional components configuration file. Components described in this file are dynamically included in robot's URDF. Available options are described in [the manual](https://husarion.com/manuals/panther/panther-options). <br/> ***string:*** [`components.yaml`](./panther_description/config/components.yaml) |
+| ✅   | ✅   | `controller_config_path`     | Path to controller configuration file. A path to custom configuration can be specified here. <br/> ***string:*** [`{wheel_type}_controller.yaml`](./husarion_ugv_controller/config/)                                                                                                                               |
 | ✅   | ✅   | `disable_manager`            | Enable or disable manager_bt_node. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                       |
 | ✅   | ✅   | `fuse_gps`                   | Include GPS for data fusion. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                             |
-| ❌   | ✅   | `gz_bridge_config_path`      | Path to the parameter_bridge configuration file. <br/> ***string:*** [`gz_bridge.yaml`](./panther_gazebo/config/gz_bridge.yaml)                                                                                                                                                                                    |
+| ❌   | ✅   | `gz_bridge_config_path`      | Path to the parameter_bridge configuration file. <br/> ***string:*** [`gz_bridge.yaml`](./husarion_ugv_gazebo/config/gz_bridge.yaml)                                                                                                                                                                               |
 | ❌   | ✅   | `gz_gui`                     | Run simulation with specific GUI layout. <br/> ***string:*** [`teleop.config`](https://github.com/husarion/husarion_gz_worlds/blob/main/config/teleop.config)                                                                                                                                                      |
 | ❌   | ✅   | `gz_headless_mode`           | Run the simulation in headless mode. Useful when a GUI is not needed or to reduce the number of calculations. <br/> ***bool:*** `False`                                                                                                                                                                            |
 | ❌   | ✅   | `gz_log_level`               | Adjust the level of console output. <br/> ***int:*** `1` (choices: `0`, `1`, `2`, `3`, `4`)                                                                                                                                                                                                                        |
 | ❌   | ✅   | `gz_world`                   | Absolute path to SDF world file. <br/> ***string:*** [`husarion_world.sdf`](https://github.com/husarion/husarion_gz_worlds/blob/main/worlds/husarion_world.sdf)                                                                                                                                                    |
 | ✅   | ✅   | `launch_nmea_gps`            | Whether to launch the NMEA NavSat driver node. Advisable when the robot is equipped with the [ANT02](https://husarion.com/manuals/panther/panther-options/#ant02---wi-fi--lte--gps). <br/> ***bool:*** `False`                                                                                                     |
-| ✅   | ✅   | `lights_bt_project_path`     | Path to BehaviorTree project file, responsible for lights management. <br/> ***string:*** [`PantherLightsBT.btproj`](./panther_manager/behavior_trees/PantherLightsBT.btproj)                                                                                                                                      |
-| ✅   | ✅   | `localization_config_path`   | Specify the path to the localization configuration file. <br/> ***string:*** [`relative_localization.yaml`](./panther_localization/config/relative_localization.yaml)                                                                                                                                              |
+| ✅   | ✅   | `lights_bt_project_path`     | Path to BehaviorTree project file, responsible for lights management. <br/> ***string:*** [`LightsBT.btproj`](./husarion_ugv_manager/behavior_trees/LightsBT.btproj)                                                                                                                                 |
+| ✅   | ✅   | `localization_config_path`   | Specify the path to the localization configuration file. <br/> ***string:*** [`relative_localization.yaml`](./husarion_ugv_localization/config/relative_localization.yaml)                                                                                                                                         |
 | ✅   | ✅   | `localization_mode`          | Specifies the localization mode:  <br/>- 'relative' `odometry/filtered` data is relative to the initial position and orientation. <br/>- 'enu' `odometry/filtered` data is relative to initial position and ENU (East North Up) orientation. <br/> ***string:*** `relative` (choices: `relative`, `enu`)           |
 | ✅   | ✅   | `namespace`                  | Add namespace to all launched nodes. <br/> ***string:*** `env(ROBOT_NAMESPACE)`                                                                                                                                                                                                                                    |
-| ✅   | ✅   | `publish_robot_state`        | Whether to publish the default Panther robot description. <br/> ***bool:*** `True`                                                                                                                                                                                                                                 |
-| ❌   | ✅   | `robot_model`                | Specify robot model type. <br/> ***string:*** `env(ROBOT_MODEL)` if not specified `panther` (choices: `lynx`, `panther`)                                                                                                                                                                                           |
-| ✅   | ✅   | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`PantherSafetyBT.btproj`](./panther_manager/behavior_trees/PantherSafetyBT.btproj)                                                                                                                         |
-| ✅   | ✅   | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts.yaml`](./panther_manager/config/shutdown_hosts.yaml)                                                                                                                                                                     |
+| ✅   | ✅   | `publish_robot_state`        | Whether to publish the default URDF of specified robot. <br/> ***bool:*** `True`                                                                                                                                                                                                                                   |
+| ❌   | ✅   | `robot_model`                | Specify robot model type. <br/> ***string:*** `env(ROBOT_MODEL)` (choices: `lynx`, `panther`)                                                                                                                                                                                                                      |
+| ✅   | ✅   | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`SafetyBT.btproj`](./husarion_ugv_manager/behavior_trees/SafetyBT.btproj)                                                                                                                    |
+| ✅   | ✅   | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts.yaml`](./husarion_ugv_manager/config/shutdown_hosts.yaml)                                                                                                                                                                |
 | ✅   | ✅   | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                    |
 | ✅   | ✅   | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                              |
 | ✅   | ✅   | `user_led_animations_path`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                    |
@@ -121,7 +121,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 
 > [!TIP]
 >
-> To read the arguments for individual packages, add the `-s` flag to the `ros2 launch` command (e.g. `ros2 launch panther_bringup bringup.launch.py ​​-s`)
+> To read the arguments for individual packages, add the `-s` flag to the `ros2 launch` command (e.g. `ros2 launch husarion_ugv_bringup bringup.launch.py ​​-s`)
 
 ## Developer Info
 
diff --git a/ROS_API.md b/ROS_API.md
index 2685fb45..f29c0664 100644
--- a/ROS_API.md
+++ b/ROS_API.md
@@ -3,9 +3,9 @@
 > [!IMPORTANT]
 > **Beta Release**
 >
-> Please be advised that the software you are about to use is a Beta version of the ROS 2 Driver for Panther. It is functional, and the architecture will not change significantly. Although it has been tested by the Husarion team, some stability issues and bugs may still occur.
+> Please be advised that the software you are about to use is a Beta version of the ROS 2 Driver for Lynx and Panther. It is functional, and the architecture will not change significantly. Although it has been tested by the Husarion team, some stability issues and bugs may still occur.
 >
-> We would greatly appreciate your feedback regarding the Panther ROS 2 driver. You can reach us in the following ways:
+> We would greatly appreciate your feedback regarding the Husarion UGV ROS 2 driver. You can reach us in the following ways:
 >
 > - By email at: [support@husarion.com](mailto:support@husarion.com)
 > - Via our community forum: [Husarion Community](https://community.husarion.com)
@@ -13,20 +13,20 @@
 
 ## ROS 2 System Design
 
-This section describes the ROS packages in the Panther ROS system. These packages are located in the [panther_ros](https://github.com/husarion/panther_ros) GitHub repository.
+This section describes the ROS packages used in Husarion UGV. These packages are located in the [panther_ros](https://github.com/husarion/panther_ros) GitHub repository.
 
 > [!NOTE]
-> **Differences in ROS System**
+> **Hardware Compatibility**
 >
-> ROS 2 nodes differs slightly between **Panther v1.06** and **Panther v1.2+**. This is caused by internal hardware differences. Despite that, the ROS API was kept as closely matched between those revisions as possible and should be transparent in most of the use cases.
+> This package supports **Lynx v0.2+**, **Panther v1.2+**. There may be small differences between robot models. This is caused by the hardware differences. Despite that, the ROS API was kept as closely matched between those revisions as possible and should be transparent in most of the use cases.
 
 <!-- TODO: add this differences -->
 
-The default way to communicate with Panther's hardware is via the Robot Operating System (ROS). All the drivers were written in ROS 2 framework. The ROS API is provided by ROS packages found in the GitHub repository [husarion/panther_ros](https://github.com/husarion/panther_ros). These packages are responsible for accessing the hardware components of the robot.
+The default way to communicate with our robots is via the Robot Operating System (ROS). All the drivers were written in ROS 2 framework. The ROS API is provided by ROS packages found in the GitHub repository [husarion/panther_ros](https://github.com/husarion/panther_ros). These packages are responsible for accessing the hardware components of the robot.
 
-The graph below represents Panther's ROS system. Some topics and services have been excluded from the graph for the sake of clarity.
+The graph below represents Husarion UVG ROS system. Some topics and services have been excluded from the graph for the sake of clarity.
 
-![Panther ROS 2 API Diagram](.docs/panther_ros2_api.drawio.svg)
+![Husarion UVG ROS 2 System Design Diagram](.docs/ros2_system_design.drawio.svg)
 
 ## ROS Interfaces
 
@@ -43,67 +43,67 @@ Below is information about the physical robot API. For the simulation, topics an
 
 | 🤖   | 🖥️   | Node name                 | Description                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                           |
 | --- | --- | ------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| ✅   | ❌   | `battery_driver`          | Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier versions of the robot. <br/> [_panther_batter/battery_driver_node_](./panther_battery/include/panther_battery/battery_driver_node.hpp)                                                                                                                                                                                                                                                          |
-| ✅   | ✅   | `controller_manager`      | The Controller Manager performs two main functions. First, it manages controllers and their required interfaces, handling tasks like loading, activating, deactivating, and unloading. Second, it interacts with hardware components, ensuring access to their interfaces. This node manages the: `imu_broadcaster`, `joint_state_broadcaster`, `drive_controller`. <br/> _[controller_manager/controller_manager](https://github.com/ros-controls/ros2_control/blob/master/controller_manager)_                                      |
-| ✅   | ✅   | `drive_controller`        | Manages mobile robots with a differential or mecanum drive depending on the configuration. It converts velocity commands for the robot body into wheel commands for the base. It also calculates odometry from hardware feedback and shares it. <br/> _[diff_drive_controller/diff_drive_controller](https://github.com/ros-controls/ros2_controllers/tree/master/diff_drive_controller) or [mecanum_drive_controller/mecanum_drive_controller](https://github.com/husarion/husarion_controllers/tree/main/mecanum_drive_controller)_ |
-| ✅   | ✅   | `ekf_filter`              | The Extended Kalman Filter node is designed to fuse odometry data from various sources, including wheel encoders, IMU, and GPS. <br/> _[robot_localization/ekf_filter](https://github.com/cra-ros-pkg/robot_localization)_                                                                                                                                                                                                                                                                                                            |
-| ✅   | ❌   | `gps`                     | Node responsible for parsing NMEA strings and publishing standard ROS NavSat message types. <br/> _[nmea_navsat_driver/nmea_navsat_driver](https://github.com/ros-drivers/nmea_navsat_driver/tree/ros2/src/libnmea_navsat_driver)_                                                                                                                                                                                                                                                                                                    |
+| ✅   | ❌   | `battery_driver`          | Publishes battery state read from ADC unit. <br/> [*panther_batter/battery_driver_node*](./husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp)                                                                                                                                                                                                                                                                                                                                                                 |
+| ✅   | ✅   | `controller_manager`      | The Controller Manager performs two main functions. First, it manages controllers and their required interfaces, handling tasks like loading, activating, deactivating, and unloading. Second, it interacts with hardware components, ensuring access to their interfaces. This node manages the: `imu_broadcaster`, `joint_state_broadcaster`, `drive_controller`. <br/> *[controller_manager/controller_manager](https://github.com/ros-controls/ros2_control/blob/master/controller_manager)*                                      |
+| ✅   | ✅   | `drive_controller`        | Manages mobile robots with a differential or mecanum drive depending on the configuration. It converts velocity commands for the robot body into wheel commands for the base. It also calculates odometry from hardware feedback and shares it. <br/> *[diff_drive_controller/diff_drive_controller](https://github.com/ros-controls/ros2_controllers/tree/master/diff_drive_controller) or [mecanum_drive_controller/mecanum_drive_controller](https://github.com/husarion/husarion_controllers/tree/main/mecanum_drive_controller)* |
+| ✅   | ✅   | `ekf_filter`              | The Extended Kalman Filter node is designed to fuse odometry data from various sources, including wheel encoders, IMU, and GPS. <br/> *[robot_localization/ekf_filter](https://github.com/cra-ros-pkg/robot_localization)*                                                                                                                                                                                                                                                                                                            |
+| ✅   | ❌   | `gps`                     | Node responsible for parsing NMEA strings and publishing standard ROS NavSat message types. <br/> *[nmea_navsat_driver/nmea_navsat_driver](https://github.com/ros-drivers/nmea_navsat_driver/tree/ros2/src/libnmea_navsat_driver)*                                                                                                                                                                                                                                                                                                    |
+| ❌   | ✅   | `gz_bridge`               | Convert and transmit data between ROS and Gazebo <br/> *[ros_gz_bridge/parameter_bridge](https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge)*                                                                                                                                                                                                                                                                                                                                                                                |
 | ❌   | ✅   | `gz_ros2_control`         | Responsible for integrating the ros2_control controller architecture with the Gazebo simulator. <br/> [gz_ros2_control/gz_ros2_control](https://github.com/ros-controls/gz_ros2_control/tree/master/gz_ros2_control/src)                                                                                                                                                                                                                                                                                                              |
-| ❌   | ✅   | `gz_estop_gui`            | The node is part of the Gazebo GUI plugin, enabling easy E-stop state modifications directly within the simulation. <br/> [panther_gazebo/EStop](./panther_gazebo/src/gui/e_stop.cpp)                                                                                                                                                                                                                                                                                                                                                 |
-| ✅   | ❌   | `hardware_controller`     | Plugin responsible for communicating with engine controllers via the CAN bus and providing E-Stop functionalities. <br/> _[panther_hardware_interfaces/PantherSystem](./panther_hardware_interfaces/src/panther_system/)_                                                                                                                                                                                                                                                                                                             |
-| ✅   | ✅   | `imu_broadcaster`         | Publishes readings of IMU sensors. <br/> _[imu_sensor_broadcaster/imu_sensor_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/imu_sensor_broadcaster)_                                                                                                                                                                                                                                                                                                                                                       |
-| ✅   | ✅   | `joint_state_broadcaster` | Reads all state interfaces and reports them on specific topics. <br/> _[joint_state_broadcaster/joint_state_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)_                                                                                                                                                                                                                                                                                                                       |
-| ✅   | ✅   | `lights_container`        | Node for managing ROS components. This node manages: `lights_controller`, `lights_driver`. <br/>[_rclcpp_components/component_container_](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_components)                                                                                                                                                                                                                                                                                                                              |
-| ✅   | ✅   | `lights_controller`       | This node is responsible for processing animations and publishing frames to be displayed on the Husarion Panther robot Bumper Lights. <br/> [_panther_lights/LightsControllerNode_](./panther_lights/include/panther_lights/lights_controller_node.hpp)                                                                                                                                                                                                                                                                               |
-| ✅   | ❌   | `lights_driver`           | This node is responsible for displaying frames on the Husarion Panther robot's Bumper Lights. <br/> [_panther_lights/LightsDriverNode_](./panther_lights/include/panther_lights/lights_driver_node.hpp)                                                                                                                                                                                                                                                                                                                               |
-| ✅   | ✅   | `lights_manager`          | Node responsible for managing Bumper Lights animation scheduling. <br/> [panther_manager/lights_manager](./panther_manager/include/panther_manager/lights_manager_node.hpp)                                                                                                                                                                                                                                                                                                                                                           |
-| ✅   | ✅   | ⚙️ `navsat_transform` ⚙️        | It converts raw GPS data into odometry data and publishes corrected GPS positions based on sensor data at a higher frequency. Requires `launch_nmea_gps` launch argument. <br/> _[robot_localization/navsat_transform](https://github.com/cra-ros-pkg/robot_localization)_                                                                                                                                                                                                                                                                                                        |
-| ❌   | ✅   | `panther_base_gz_bridge`  | Convert and transmit data between ROS and Gazebo. <br/> _[ros_gz_bridge/parameter_bridge](https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge)_                                                                                                                                                                                                                                                                                                                                                                               |
-| ✅   | ✅   | `robot_state_publisher`   | Broadcasts a robot's state to tf2 using a provided URDF model and joint states. It updates the model and broadcasts poses for fixed and movable joints to tf2 topics. <br/> _[robot_state_publisher/robot_state_publisher](https://github.com/ros/robot_state_publisher)_                                                                                                                                                                                                                                                             |
-| ✅   | ❌   | `safety_manager`          | Node responsible for managing safety features, and software shutdown of components. <br/> _[panther_manager/safety_manager_node](./panther_manager/include/panther_manager/safety_manager_node.hpp)_                                                                                                                                                                                                                                                                                                                                  |
-| ✅   | ❌   | `system_monitor`          | Publishes system state of the Built-in Computer such as CPU usage, RAM memory usage, disk usage and CPU temperature. <br/> _[panther_diagnostic/system_monitor_node](./panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp)_                                                                                                                                                                                                                                                                                      |
+| ❌   | ✅   | `gz_estop_gui`            | The node is part of the Gazebo GUI plugin, enabling easy E-stop state modifications directly within the simulation. <br/> [husarion_ugv_gazebo/EStop](./husarion_ugv_gazebo/src/gui/e_stop.cpp)                                                                                                                                                                                                                                                                                                                                       |
+| ✅   | ❌   | `hardware_controller`     | Plugin responsible for communicating with engine controllers via the CAN bus and providing E-Stop functionalities. <br/> *[husarion_ugv_hardware_interfaces/{robot_model}System](./husarion_ugv_hardware_interfaces/src/robot_system/)*                                                                                                                                                                                                                                                                                               |
+| ✅   | ✅   | `imu_broadcaster`         | Publishes readings of IMU sensors. <br/> *[imu_sensor_broadcaster/imu_sensor_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/imu_sensor_broadcaster)*                                                                                                                                                                                                                                                                                                                                                       |
+| ✅   | ✅   | `joint_state_broadcaster` | Reads all state interfaces and reports them on specific topics. <br/> *[joint_state_broadcaster/joint_state_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)*                                                                                                                                                                                                                                                                                                                       |
+| ✅   | ✅   | `lights_container`        | Node for managing ROS components. This node manages: `lights_controller`, `lights_driver`. <br/>[*rclcpp_components/component_container*](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_components)                                                                                                                                                                                                                                                                                                                              |
+| ✅   | ✅   | `lights_controller`       | This node is responsible for processing animations and publishing frames to `light_driver` node. <br/> [*husarion_ugv_lights/LightsControllerNode*](./husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp)                                                                                                                                                                                                                                                                                                     |
+| ✅   | ❌   | `lights_driver`           | This node is responsible for displaying frames on the robot's lights. <br/> [*husarion_ugv_lights/LightsDriverNode*](./husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp)                                                                                                                                                                                                                                                                                                                                        |
+| ✅   | ✅   | `lights_manager`          | Node responsible for managing lights animation scheduling. <br/> [husarion_ugv_manager/lights_manager](./husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp)                                                                                                                                                                                                                                                                                                                                                   |
+| ✅   | ✅   | ⚙️ `navsat_transform`  ⚙️   | It converts raw GPS data into odometry data and publishes corrected GPS positions based on sensor data at a higher frequency. <br/> *[robot_localization/navsat_transform](https://github.com/cra-ros-pkg/robot_localization)*                                                                                                                                                                                                                                                                                                        |
+| ✅   | ✅   | `robot_state_publisher`   | Broadcasts a robot's state to tf2 using a provided URDF model and joint states. It updates the model and broadcasts poses for fixed and movable joints to tf2 topics. <br/> *[robot_state_publisher/robot_state_publisher](https://github.com/ros/robot_state_publisher)*                                                                                                                                                                                                                                                             |
+| ✅   | ❌   | `safety_manager`          | Node responsible for managing safety features, and software shutdown of components. <br/> *[husarion_ugv_manager/safety_manager_node](./husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp)*                                                                                                                                                                                                                                                                                                                   |
+| ✅   | ❌   | `system_monitor`          | Publishes system state of the Built-in Computer such as CPU usage, RAM memory usage, disk usage and  CPU temperature. <br/> *[husarion_ugv_diagnostics/system_monitor_node](./husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/system_monitor_node.hpp)*                                                                                                                                                                                                                                                                     |
 
 ### Topics
 
 | 🤖   | 🖥️   | Topic                         | Description                                                                                                                                                                                                                                                                                                                                                               |
 | --- | --- | ----------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| ✅   | ✅   | `battery/battery_status`      | Mean values of both batteries if Panther has two batteries. Otherwise, the state of the single battery will be published. <br/> [_sensor_msgs/BatteryState_](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html)                                                                                                                                           |
-| ✅   | ❌   | `battery/charging_status`     | Battery charging status value. <br/> [*panther_msgs/ChargingStatus*](https://github.com/husarion/panther_msgs)                                                                                                                                                                                                                                                             |
-| ✅   | ✅   | `cmd_vel`                     | Command velocity value. <br/> [_geometry_msgs/Twist_](https://docs.ros2.org/latest/api/geometry_msgs/msg/Twist.html)                                                                                                                                                                                                                                                       |
-| ✅   | ✅   | `diagnostics`                 | Diagnostic data. <br/> [_diagnostic_msgs/DiagnosticArray_](https://docs.ros2.org/latest/api/diagnostic_msgs/msg/DiagnosticArray.html)                                                                                                                                                                                                                                      |
-| ✅   | ✅   | `dynamic_joint_states`        | Information about the state of various movable joints in a robotic system. <br/> [_control_msgs/DynamicJointState_](https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/DynamicJointState.msg)                                                                                                                                                       |
-| ✅   | ✅   | ⚙️ `gps/filtered` ⚙️                | Filtered GPS position after fusing odometry data. <br/> [_sensor_msgs/NavSatFix_](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html)                                                                                                                                                                                                                         |
-| ✅   | ✅   | ⚙️ `gps/fix` ⚙️                     | Raw GPS data. <br/> [_sensor_msgs/NavSatFix_](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html)                                                                                                                                                                                                                                                             |
-| ✅   | ❌   | `gps/time_reference`          | The timestamp from the GPS device. <br/> [_sensor_msgs/TimeReference_](https://docs.ros2.org/latest/api/sensor_msgs/msg/TimeReference.html)                                                                                                                                                                                                                                |
-| ✅   | ❌   | `gps/vel`                     | Velocity output from the GPS device. <br/> [_geometry_msgs/TwistStamped_](https://docs.ros2.org/latest/api/geometry_msgs/msg/TwistStamped.html)                                                                                                                                                                                                                            |
-| ✅   | ✅   | `hardware/e_stop`             | Current E-stop state. <br/> [_std_msgs/Bool_](https://docs.ros2.org/latest/api/std_msgs/msg/Bool.html).                                                                                                                                                                                                                                                                    |
-| ✅   | ❌   | `hardware/io_state`           | Current IO state. <br/> [_panther_msgs/IOState_](https://github.com/husarion/panther_msgs)                                                                                                                                                                                                                                                                                 |
-| ✅   | ✅   | `hardware/robot_driver_state` | Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data. <br/> [_panther_msgs/RobotDriverState_](https://github.com/husarion/panther_msgs)                                                                                                                                                                                    |
-| ✅   | ✅   | `imu/data`                    | Filtered IMU data. <br/> [_sensor_msgs/Imu_](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)                                                                                                                                                                                                                                                                    |
-| ✅   | ✅   | `joint_states`                | Provides information about the state of various joints in a robotic system. <br/> [_sensor_msgs/JointState_](https://docs.ros2.org/latest/api/sensor_msgs/msg/JointState.html)                                                                                                                                                                                             |
-| ✅   | ✅   | `lights/channel_1_frame`      | Frame to be displayed on robot Front Bumper Lights. <br/> [_sensor_msgs/Image_](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html)                                                                                                                                                                                                                               |
-| ✅   | ✅   | `lights/channel_2_frame`      | Frame to be displayed on robot Rear Bumper Lights. <br/> [_sensor_msgs/Image_](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html)                                                                                                                                                                                                                                |
-| ✅   | ✅   | `localization/set_pose`       | Sets the pose of the EKF node. <br/> [_geometry_msgs/PoseWithCovarianceStamped_](https://docs.ros2.org/latest/api/geometry_msgs/msg/PoseWithCovarianceStamped.html)                                                                                                                                                                                                        |
-| ✅   | ✅   | `odometry/filtered`           | Contains information about the filtered position and orientation. When `localization_mode` is `relative`, the position and orientation are relative to the starting point. When `localization_mode` is `enu`, the orientation is relative to the east-north-up (ENU) coordinates. <br/> [_nav_msgs/Odometry_](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) |
-| ✅   | ✅   | `odometry/wheels`             | Robot odometry calculated from wheels. <br/> [_nav_msgs/Odometry_](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html)                                                                                                                                                                                                                                            |
-| ✅   | ✅   | `robot_description`           | Contains information about robot description from URDF file. <br/> [_std_msgs/String_](https://docs.ros2.org/latest/api/std_msgs/msg/String.html)                                                                                                                                                                                                                         |
-| ✅   | ❌   | `system_status`               | State of the system, including Built-in Computer's CPU temperature and load. <br/> [_panther_msgs/SystemStatus_](https://github.com/husarion/panther_msgs)                                                                                                                                                                                                                |
-| ✅   | ✅   | `tf`                          | Transforms of robot system. <br/> [_tf2_msgs/TFMessage_](https://docs.ros2.org/latest/api/tf2_msgs/msg/TFMessage.html)                                                                                                                                                                                                                                                     |
-| ✅   | ✅   | `tf_static`                   | Static transforms of robot system. <br/> [_tf2_msgs/TFMessage_](https://docs.ros2.org/latest/api/tf2_msgs/msg/TFMessage.html)                                                                                                                                                                                                                                              |
+| ✅   | ✅   | `battery/battery_status`      | Mean values of both batteries will be published if the robot has two batteries. Otherwise, the state of the single battery will be published.<br/> [*sensor_msgs/BatteryState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html)                                                                                                                       |
+| ✅   | ❌   | `battery/charging_status`     | Battery charging status value.<br/> [*panther_msgs/ChargingStatus*](https://github.com/husarion/panther_msgs)                                                                                                                                                                                                                                                             |
+| ✅   | ✅   | `cmd_vel`                     | Command velocity value.<br/> [*geometry_msgs/Twist*](https://docs.ros2.org/latest/api/geometry_msgs/msg/Twist.html)                                                                                                                                                                                                                                                       |
+| ✅   | ✅   | `diagnostics`                 | Diagnostic data.<br/> [*diagnostic_msgs/DiagnosticArray*](https://docs.ros2.org/latest/api/diagnostic_msgs/msg/DiagnosticArray.html)                                                                                                                                                                                                                                      |
+| ✅   | ✅   | `dynamic_joint_states`        | Information about the state of various movable joints in a robotic system.<br/> [*control_msgs/DynamicJointState*](https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/DynamicJointState.msg)                                                                                                                                                       |
+| ✅   | ✅   | ⚙️ `gps/filtered` ⚙️            | Filtered GPS position after fusing odometry data.<br/> [*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html)                                                                                                                                                                                                                         |
+| ✅   | ✅   | ⚙️ `gps/fix` ⚙️                 | Raw GPS data.<br/> [*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html)                                                                                                                                                                                                                                                             |
+| ✅   | ❌   | `gps/time_reference`          | The timestamp from the GPS device.<br/> [*sensor_msgs/TimeReference*](https://docs.ros2.org/latest/api/sensor_msgs/msg/TimeReference.html)                                                                                                                                                                                                                                |
+| ✅   | ❌   | `gps/vel`                     | Velocity output from the GPS device.<br/> [*geometry_msgs/TwistStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/TwistStamped.html)                                                                                                                                                                                                                            |
+| ✅   | ✅   | `hardware/e_stop`             | Current E-stop state.<br/> [*std_msgs/Bool*](https://docs.ros2.org/latest/api/std_msgs/msg/Bool.html).                                                                                                                                                                                                                                                                    |
+| ✅   | ❌   | `hardware/io_state`           | Current IO state.<br/> [*panther_msgs/IOState*](https://github.com/husarion/panther_msgs)                                                                                                                                                                                                                                                                                 |
+| ✅   | ✅   | `hardware/robot_driver_state` | Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.<br/> [*panther_msgs/RobotDriverState*](https://github.com/husarion/panther_msgs)                                                                                                                                                                                    |
+| ✅   | ✅   | `imu/data`                    | Filtered IMU data.<br/> [*sensor_msgs/Imu*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)                                                                                                                                                                                                                                                                    |
+| ✅   | ✅   | `joint_states`                | Provides information about the state of various joints in a robotic system.<br/> [*sensor_msgs/JointState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/JointState.html)                                                                                                                                                                                             |
+| ✅   | ✅   | `lights/channel_1_frame`      | Frame to be displayed on robot Front Bumper Lights.<br/> [*sensor_msgs/Image*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html)                                                                                                                                                                                                                               |
+| ✅   | ✅   | `lights/channel_2_frame`      | Frame to be displayed on robot Rear Bumper Lights.<br/> [*sensor_msgs/Image*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html)                                                                                                                                                                                                                                |
+| ✅   | ✅   | `localization/set_pose`       | Sets the pose of the EKF node.<br/> [*geometry_msgs/PoseWithCovarianceStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/PoseWithCovarianceStamped.html)                                                                                                                                                                                                        |
+| ✅   | ✅   | `odometry/filtered`           | Contains information about the filtered position and orientation. When `localization_mode` is `relative`, the position and orientation are relative to the starting point. When `localization_mode` is `enu`, the orientation is relative to the east-north-up (ENU) coordinates.<br/> [*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) |
+| ✅   | ✅   | `odometry/wheels`             | Robot odometry calculated from wheels.<br/> [*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html)                                                                                                                                                                                                                                            |
+| ✅   | ✅   | `robot_description`           | Contains information about robot description from URDF file. <br/> [*std_msgs/String*](https://docs.ros2.org/latest/api/std_msgs/msg/String.html)                                                                                                                                                                                                                         |
+| ✅   | ❌   | `system_status`               | State of the system, including Built-in Computer's CPU temperature and load. <br/> [*panther_msgs/SystemStatus*](https://github.com/husarion/panther_msgs)                                                                                                                                                                                                                |
+| ✅   | ✅   | `tf`                          | Transforms of robot system.<br/> [*tf2_msgs/TFMessage*](https://docs.ros2.org/latest/api/tf2_msgs/msg/TFMessage.html)                                                                                                                                                                                                                                                     |
+| ✅   | ✅   | `tf_static`                   | Static transforms of robot system.<br/> [*tf2_msgs/TFMessage*](https://docs.ros2.org/latest/api/tf2_msgs/msg/TFMessage.html)                                                                                                                                                                                                                                              |
 
 #### Hidden topics
 
-| 🤖   | 🖥️   | Topic                           | Description                                                                                                                                                           |
-| --- | --- | ------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| 🤖   | 🖥️   | Topic                           | Description                                                                                                                                                            |
+| --- | --- | ------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
 | ✅   | ❌   | `_battery/battery_1_status_raw` | First battery raw state. <br/> [_sensor_msgs/BatteryState_](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html)                                        |
 | ✅   | ❌   | `_battery/battery_2_status_raw` | Second battery raw state. Published if second battery detected. <br/> [_sensor_msgs/BatteryState_](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) |
 | ✅   | ❌   | `_gps/heading`                  | Not supported for current configuration. <br/> [_geometry_msgs/QuaternionStamped_](https://docs.ros2.org/latest/api/geometry_msgs/msg/QuaternionStamped.html)          |
-| ✅ | ✅   | ⚙️ `_odometry/gps` ⚙️                 | Transformed raw GPS data to odometry format. <br/> [_nav_msgs/Odometry_](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html)                                  |
+| ✅   | ✅   | ⚙️ `_odometry/gps` ⚙️             | Transformed raw GPS data to odometry format. <br/> [_nav_msgs/Odometry_](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html)                                  |
 
 ### Services
 
-| 🤖   | 🖥️   | Service                                           | Description                                                                                                                                                                                            |
-| --- | --- | ------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| 🤖   | 🖥️   | Service                                           | Description                                                                                                                                                                                             |
+| --- | --- | ------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
 | ✅   | ✅   | `controller_manager/configure_controller`         | Manage lifecycle transition. <br/> [controller_manager_msgs/srv/ConfigureController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)                                  |
 | ✅   | ✅   | `controller_manager/list_controller_types`        | Output the available controller types and their base classes. <br/> [controller_manager_msgs/srv/ListControllerTypes](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) |
 | ✅   | ✅   | `controller_manager/list_controllers`             | Output the list of loaded controllers, their type and status. <br/> [controller_manager_msgs/srv/ListControllers](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs)     |
diff --git a/panther/CHANGELOG.rst b/husarion_ugv/CHANGELOG.rst
similarity index 100%
rename from panther/CHANGELOG.rst
rename to husarion_ugv/CHANGELOG.rst
diff --git a/panther/CMakeLists.txt b/husarion_ugv/CMakeLists.txt
similarity index 80%
rename from panther/CMakeLists.txt
rename to husarion_ugv/CMakeLists.txt
index 626fc36a..2c2d0e25 100644
--- a/panther/CMakeLists.txt
+++ b/husarion_ugv/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10.2)
-project(panther)
+project(husarion_ugv)
 
 find_package(ament_cmake REQUIRED)
 
diff --git a/husarion_ugv/README.md b/husarion_ugv/README.md
new file mode 100644
index 00000000..bc079c16
--- /dev/null
+++ b/husarion_ugv/README.md
@@ -0,0 +1,3 @@
+# husarion_ugv
+
+ROS 2 Metapackage composing basic functionalities of the Husarion UGV robot with VCS Tool yaml files directing to external robot dependencies.
diff --git a/panther/panther_hardware.repos b/husarion_ugv/hardware_deps.repos
similarity index 100%
rename from panther/panther_hardware.repos
rename to husarion_ugv/hardware_deps.repos
diff --git a/panther/package.xml b/husarion_ugv/package.xml
similarity index 71%
rename from panther/package.xml
rename to husarion_ugv/package.xml
index f849f567..e426e387 100644
--- a/panther/package.xml
+++ b/husarion_ugv/package.xml
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>panther</name>
+  <name>husarion_ugv</name>
   <version>2.1.1</version>
-  <description>Meta package that contains all packages of Panther</description>
+  <description>Meta package that contains all packages of Husarion UGV (Unmanned Ground Vehicle)</description>
   <maintainer email="support@husarion.com">Husarion</maintainer>
   <license>Apache License 2.0</license>
 
@@ -15,8 +15,8 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>panther_bringup</depend>
-  <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_gazebo</depend>
+  <depend>husarion_ugv_bringup</depend>
+  <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">husarion_ugv_gazebo</depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/panther/panther_simulation.repos b/husarion_ugv/simulation_deps.repos
similarity index 100%
rename from panther/panther_simulation.repos
rename to husarion_ugv/simulation_deps.repos
diff --git a/panther_battery/CHANGELOG.rst b/husarion_ugv_battery/CHANGELOG.rst
similarity index 100%
rename from panther_battery/CHANGELOG.rst
rename to husarion_ugv_battery/CHANGELOG.rst
diff --git a/panther_battery/CMakeLists.txt b/husarion_ugv_battery/CMakeLists.txt
similarity index 97%
rename from panther_battery/CMakeLists.txt
rename to husarion_ugv_battery/CMakeLists.txt
index 4e12bab8..d928de57 100644
--- a/panther_battery/CMakeLists.txt
+++ b/husarion_ugv_battery/CMakeLists.txt
@@ -1,18 +1,18 @@
 cmake_minimum_required(VERSION 3.10.2)
-project(panther_battery)
+project(husarion_ugv_battery)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
 set(PACKAGE_DEPENDENCIES ament_cmake diagnostic_updater panther_msgs
-                         panther_utils rclcpp sensor_msgs)
+                         husarion_ugv_utils rclcpp sensor_msgs)
 
 foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
   find_package(${PACKAGE} REQUIRED)
 endforeach()
 
-include_directories(include ${panther_utils_INCLUDE_DIRS})
+include_directories(include ${husarion_ugv_utils_INCLUDE_DIRS})
 
 add_executable(
   battery_driver_node
diff --git a/panther_battery/README.md b/husarion_ugv_battery/README.md
similarity index 75%
rename from panther_battery/README.md
rename to husarion_ugv_battery/README.md
index d591938d..a6fcc35f 100644
--- a/panther_battery/README.md
+++ b/husarion_ugv_battery/README.md
@@ -1,6 +1,6 @@
-# panther_battery
+# husarion_ugv_battery
 
-The package containing nodes monitoring and publishing the internal battery state of the Husarion Panther robot.
+The package containing nodes monitoring and publishing the internal battery state of the Husarion UGV.
 
 ## Launch Files
 
@@ -12,13 +12,13 @@ This package contains:
 
 ### battery_driver_node
 
-Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier.versions of the robot.
+Publishes battery state read from ADC unit.
 
 #### Publishes
 
 - `_battery/battery_1_status_raw` [*sensor_msgs/BatteryState*]: First battery raw state.
 - `_battery/battery_2_status_raw` [*sensor_msgs/BatteryState*]: Second battery raw state. Published if second battery detected.
-- `battery/battery_status` [*sensor_msgs/BatteryState*]: Mean values of both batteries if Panther has two batteries. Otherwise, the state of the single battery will be published.
+- `battery/battery_status` [*sensor_msgs/BatteryState*]: Mean values of both batteries if robot has two batteries. Otherwise, the state of the single battery will be published.
 - `battery/charging_status` [*panther_msgs/ChargingStatus*]: Battery charging status.
 - `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Battery diagnostic messages.
 
@@ -29,11 +29,11 @@ Publishes battery state read from ADC unit for Panther version 1.2 and above, or
 
 #### Parameters
 
-- `~/adc/device0` [*string*, default: **/sys/bus/iio/devices/iio:device0**]: ADC nr 0 IIO device. Used with Panther version 1.2 and above.
-- `~/adc/device1` [*string*, default: **/sys/bus/iio/devices/iio:device1**]: ADC nr 1 IIO device. Used with Panther version 1.2 and above.
-- `~/adc/ma_window_len/charge` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery charge readings. Used with Panther version 1.2 and above.
-- `~/adc/ma_window_len/temp` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery temperature readings. Used with Panther version 1.2 and above.
+- `~/adc/device0` [*string*, default: **/sys/bus/iio/devices/iio:device0**]: ADC number 0 IIO device.
+- `~/adc/device1` [*string*, default: **/sys/bus/iio/devices/iio:device1**]: ADC number 1 IIO device.
+- `~/adc/ma_window_len/charge` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery charge readings.
+- `~/adc/ma_window_len/temp` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery temperature readings.
 - `~/battery_timeout` [*float*, default: **1.0**]: Specifies the timeout in seconds. If the node fails to read battery data exceeding this duration, the node will publish an unknown battery state.
 - `~/ma_window_len/voltage` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery voltage readings.
 - `~/ma_window_len/current` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery current readings.
-- `~/roboteq/driver_state_timeout` [*float*, default: **0.2**]: Specifies timeout in seconds after which driver state messages will be considered old. Used with Panther version 1.06 and earlier.
+- `~/roboteq/driver_state_timeout` [*float*, default: **0.2**]: Specifies timeout in seconds after which driver state messages will be considered old (deprecated).
diff --git a/panther_battery/include/panther_battery/adc_data_reader.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/adc_data_reader.hpp
similarity index 90%
rename from panther_battery/include/panther_battery/adc_data_reader.hpp
rename to husarion_ugv_battery/include/husarion_ugv_battery/adc_data_reader.hpp
index 08dca0d0..59e1fb57 100644
--- a/panther_battery/include/panther_battery/adc_data_reader.hpp
+++ b/husarion_ugv_battery/include/husarion_ugv_battery/adc_data_reader.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_BATTERY_ADC_DATA_READER_HPP_
-#define PANTHER_BATTERY_ADC_DATA_READER_HPP_
+#ifndef HUSARION_UGV_BATTERY_ADC_DATA_READER_HPP_
+#define HUSARION_UGV_BATTERY_ADC_DATA_READER_HPP_
 
 #include <filesystem>
 #include <fstream>
 #include <stdexcept>
 #include <string>
 
-namespace panther_battery
+namespace husarion_ugv_battery
 {
 
 class ADCDataReader
@@ -75,6 +75,6 @@ class ADCDataReader
   const std::filesystem::path device_path_;
 };
 
-}  // namespace panther_battery
+}  // namespace husarion_ugv_battery
 
-#endif  // PANTHER_BATTERY_ADC_DATA_READER_HPP_
+#endif  // HUSARION_UGV_BATTERY_ADC_DATA_READER_HPP_
diff --git a/panther_battery/include/panther_battery/battery/adc_battery.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery/adc_battery.hpp
similarity index 83%
rename from panther_battery/include/panther_battery/battery/adc_battery.hpp
rename to husarion_ugv_battery/include/husarion_ugv_battery/battery/adc_battery.hpp
index 6aebfa09..94dd428d 100644
--- a/panther_battery/include/panther_battery/battery/adc_battery.hpp
+++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery/adc_battery.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_BATTERY_BATTERY_ADC_BATTERY_HPP_
-#define PANTHER_BATTERY_BATTERY_ADC_BATTERY_HPP_
+#ifndef HUSARION_UGV_BATTERY_BATTERY_ADC_BATTERY_HPP_
+#define HUSARION_UGV_BATTERY_BATTERY_ADC_BATTERY_HPP_
 
 #include <cstdint>
 #include <functional>
@@ -21,10 +21,10 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_battery/battery/battery.hpp"
-#include "panther_utils/moving_average.hpp"
+#include "husarion_ugv_battery/battery/battery.hpp"
+#include "husarion_ugv_utils/moving_average.hpp"
 
-namespace panther_battery
+namespace husarion_ugv_battery
 {
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
@@ -89,12 +89,12 @@ class ADCBattery : public Battery
   const std::function<float()> ReadTemp;
   const std::function<float()> ReadCharge;
 
-  std::unique_ptr<panther_utils::MovingAverage<float>> voltage_ma_;
-  std::unique_ptr<panther_utils::MovingAverage<float>> current_ma_;
-  std::unique_ptr<panther_utils::MovingAverage<float>> temp_ma_;
-  std::unique_ptr<panther_utils::MovingAverage<float>> charge_ma_;
+  std::unique_ptr<husarion_ugv_utils::MovingAverage<float>> voltage_ma_;
+  std::unique_ptr<husarion_ugv_utils::MovingAverage<float>> current_ma_;
+  std::unique_ptr<husarion_ugv_utils::MovingAverage<float>> temp_ma_;
+  std::unique_ptr<husarion_ugv_utils::MovingAverage<float>> charge_ma_;
 };
 
-}  // namespace panther_battery
+}  // namespace husarion_ugv_battery
 
-#endif  // PANTHER_BATTERY_BATTERY_ADC_BATTERY_HPP_
+#endif  // HUSARION_UGV_BATTERY_BATTERY_ADC_BATTERY_HPP_
diff --git a/panther_battery/include/panther_battery/battery/battery.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery/battery.hpp
similarity index 95%
rename from panther_battery/include/panther_battery/battery/battery.hpp
rename to husarion_ugv_battery/include/husarion_ugv_battery/battery/battery.hpp
index eb067aa6..7e461cfa 100644
--- a/panther_battery/include/panther_battery/battery/battery.hpp
+++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery/battery.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_BATTERY_BATTERY_BATTERY_HPP_
-#define PANTHER_BATTERY_BATTERY_BATTERY_HPP_
+#ifndef HUSARION_UGV_BATTERY_BATTERY_BATTERY_HPP_
+#define HUSARION_UGV_BATTERY_BATTERY_BATTERY_HPP_
 
 #include <algorithm>
 #include <limits>
@@ -27,7 +27,7 @@
 
 #include "panther_msgs/msg/charging_status.hpp"
 
-namespace panther_battery
+namespace husarion_ugv_battery
 {
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
@@ -127,6 +127,6 @@ class Battery
   ChargingStatusMsg charging_status_;
 };
 
-}  // namespace panther_battery
+}  // namespace husarion_ugv_battery
 
-#endif  // PANTHER_BATTERY_BATTERY_BATTERY_HPP_
+#endif  // HUSARION_UGV_BATTERY_BATTERY_BATTERY_HPP_
diff --git a/panther_battery/include/panther_battery/battery/roboteq_battery.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery/roboteq_battery.hpp
similarity index 81%
rename from panther_battery/include/panther_battery/battery/roboteq_battery.hpp
rename to husarion_ugv_battery/include/husarion_ugv_battery/battery/roboteq_battery.hpp
index 34d1d3cd..0b785f79 100644
--- a/panther_battery/include/panther_battery/battery/roboteq_battery.hpp
+++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery/roboteq_battery.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_
-#define PANTHER_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_
+#ifndef HUSARION_UGV_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_
+#define HUSARION_UGV_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_
 
 #include <cstdint>
 #include <functional>
@@ -24,10 +24,10 @@
 #include "panther_msgs/msg/driver_state_named.hpp"
 #include "panther_msgs/msg/robot_driver_state.hpp"
 
-#include "panther_battery/battery/battery.hpp"
-#include "panther_utils/moving_average.hpp"
+#include "husarion_ugv_battery/battery/battery.hpp"
+#include "husarion_ugv_utils/moving_average.hpp"
 
-namespace panther_battery
+namespace husarion_ugv_battery
 {
 
 using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
@@ -73,10 +73,10 @@ class RoboteqBattery : public Battery
   float current_raw_;
   RobotDriverStateMsg::SharedPtr driver_state_;
 
-  std::unique_ptr<panther_utils::MovingAverage<float>> voltage_ma_;
-  std::unique_ptr<panther_utils::MovingAverage<float>> current_ma_;
+  std::unique_ptr<husarion_ugv_utils::MovingAverage<float>> voltage_ma_;
+  std::unique_ptr<husarion_ugv_utils::MovingAverage<float>> current_ma_;
 };
 
-}  // namespace panther_battery
+}  // namespace husarion_ugv_battery
 
-#endif  // PANTHER_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_
+#endif  // HUSARION_UGV_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_
diff --git a/panther_battery/include/panther_battery/battery_driver_node.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp
similarity index 80%
rename from panther_battery/include/panther_battery/battery_driver_node.hpp
rename to husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp
index 0832bc03..1bba8b99 100644
--- a/panther_battery/include/panther_battery/battery_driver_node.hpp
+++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_BATTERY_BATTERY_DRIVER_NODE_HPP_
-#define PANTHER_BATTERY_BATTERY_DRIVER_NODE_HPP_
+#ifndef HUSARION_UGV_BATTERY_BATTERY_DRIVER_NODE_HPP_
+#define HUSARION_UGV_BATTERY_BATTERY_DRIVER_NODE_HPP_
 
 #include <memory>
 #include <string>
@@ -23,11 +23,11 @@
 
 #include "panther_msgs/msg/robot_driver_state.hpp"
 
-#include "panther_battery/adc_data_reader.hpp"
-#include "panther_battery/battery/battery.hpp"
-#include "panther_battery/battery_publisher/battery_publisher.hpp"
+#include "husarion_ugv_battery/adc_data_reader.hpp"
+#include "husarion_ugv_battery/battery/battery.hpp"
+#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp"
 
-namespace panther_battery
+namespace husarion_ugv_battery
 {
 
 using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
@@ -60,6 +60,6 @@ class BatteryDriverNode : public rclcpp::Node
   std::shared_ptr<diagnostic_updater::Updater> diagnostic_updater_;
 };
 
-}  // namespace panther_battery
+}  // namespace husarion_ugv_battery
 
-#endif  // PANTHER_BATTERY_BATTERY_DRIVER_NODE_HPP_
+#endif  // HUSARION_UGV_BATTERY_BATTERY_DRIVER_NODE_HPP_
diff --git a/panther_battery/include/panther_battery/battery_publisher/battery_publisher.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/battery_publisher.hpp
similarity index 88%
rename from panther_battery/include/panther_battery/battery_publisher/battery_publisher.hpp
rename to husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/battery_publisher.hpp
index 688f1899..9258b9a5 100644
--- a/panther_battery/include/panther_battery/battery_publisher/battery_publisher.hpp
+++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/battery_publisher.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_
-#define PANTHER_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_
+#ifndef HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_
+#define HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_
 
 #include <memory>
 
@@ -25,7 +25,7 @@
 #include "panther_msgs/msg/charging_status.hpp"
 #include "panther_msgs/msg/io_state.hpp"
 
-namespace panther_battery
+namespace husarion_ugv_battery
 {
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
@@ -71,6 +71,6 @@ class BatteryPublisher
   rclcpp::Subscription<IOStateMsg>::SharedPtr io_state_sub_;
 };
 
-}  // namespace panther_battery
+}  // namespace husarion_ugv_battery
 
-#endif  // PANTHER_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_
+#endif  // HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_
diff --git a/panther_battery/include/panther_battery/battery_publisher/dual_battery_publisher.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp
similarity index 84%
rename from panther_battery/include/panther_battery/battery_publisher/dual_battery_publisher.hpp
rename to husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp
index 9890465f..738d7766 100644
--- a/panther_battery/include/panther_battery/battery_publisher/dual_battery_publisher.hpp
+++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp
@@ -12,18 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_
-#define PANTHER_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_
+#ifndef HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_
+#define HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_
 
 #include <cstdint>
 #include <memory>
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_battery/battery/battery.hpp"
-#include "panther_battery/battery_publisher/battery_publisher.hpp"
+#include "husarion_ugv_battery/battery/battery.hpp"
+#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp"
 
-namespace panther_battery
+namespace husarion_ugv_battery
 {
 
 class DualBatteryPublisher : public BatteryPublisher
@@ -66,6 +66,6 @@ class DualBatteryPublisher : public BatteryPublisher
   rclcpp::Publisher<ChargingStatusMsg>::SharedPtr charging_status_pub_;
 };
 
-}  // namespace panther_battery
+}  // namespace husarion_ugv_battery
 
-#endif  // PANTHER_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_
+#endif  // HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_
diff --git a/panther_battery/include/panther_battery/battery_publisher/single_battery_publisher.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp
similarity index 78%
rename from panther_battery/include/panther_battery/battery_publisher/single_battery_publisher.hpp
rename to husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp
index 29be9bc5..f1e65ecf 100644
--- a/panther_battery/include/panther_battery/battery_publisher/single_battery_publisher.hpp
+++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp
@@ -12,17 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_
-#define PANTHER_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_
+#ifndef HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_
+#define HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_
 
 #include <memory>
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_battery/battery/battery.hpp"
-#include "panther_battery/battery_publisher/battery_publisher.hpp"
+#include "husarion_ugv_battery/battery/battery.hpp"
+#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp"
 
-namespace panther_battery
+namespace husarion_ugv_battery
 {
 
 class SingleBatteryPublisher : public BatteryPublisher
@@ -51,6 +51,6 @@ class SingleBatteryPublisher : public BatteryPublisher
   rclcpp::Publisher<ChargingStatusMsg>::SharedPtr charging_status_pub_;
 };
 
-}  // namespace panther_battery
+}  // namespace husarion_ugv_battery
 
-#endif  // PANTHER_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_
+#endif  // HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_
diff --git a/panther_battery/launch/battery.launch.py b/husarion_ugv_battery/launch/battery.launch.py
similarity index 97%
rename from panther_battery/launch/battery.launch.py
rename to husarion_ugv_battery/launch/battery.launch.py
index 51139e50..d8327e33 100644
--- a/panther_battery/launch/battery.launch.py
+++ b/husarion_ugv_battery/launch/battery.launch.py
@@ -29,7 +29,7 @@ def generate_launch_description():
     )
 
     battery_driver_node = Node(
-        package="panther_battery",
+        package="husarion_ugv_battery",
         executable="battery_driver_node",
         name="battery_driver",
         namespace=namespace,
diff --git a/panther_battery/package.xml b/husarion_ugv_battery/package.xml
similarity index 93%
rename from panther_battery/package.xml
rename to husarion_ugv_battery/package.xml
index 8e67a1b9..70f025d7 100644
--- a/panther_battery/package.xml
+++ b/husarion_ugv_battery/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>panther_battery</name>
+  <name>husarion_ugv_battery</name>
   <version>2.1.1</version>
   <description>Nodes monitoring the battery state of Husarion Panhter robot</description>
   <maintainer email="support@husarion.com">Husarion</maintainer>
@@ -17,8 +17,8 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>diagnostic_updater</depend>
+  <depend>husarion_ugv_utils</depend>
   <depend>panther_msgs</depend>
-  <depend>panther_utils</depend>
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
 
diff --git a/panther_battery/src/battery/adc_battery.cpp b/husarion_ugv_battery/src/battery/adc_battery.cpp
similarity index 93%
rename from panther_battery/src/battery/adc_battery.cpp
rename to husarion_ugv_battery/src/battery/adc_battery.cpp
index 968f4823..cd13f25a 100644
--- a/panther_battery/src/battery/adc_battery.cpp
+++ b/husarion_ugv_battery/src/battery/adc_battery.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_battery/battery/adc_battery.hpp"
+#include "husarion_ugv_battery/battery/adc_battery.hpp"
 
 #include <chrono>
 #include <cmath>
@@ -24,9 +24,9 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_utils/moving_average.hpp"
+#include "husarion_ugv_utils/moving_average.hpp"
 
-namespace panther_battery
+namespace husarion_ugv_battery
 {
 
 ADCBattery::ADCBattery(
@@ -35,13 +35,13 @@ ADCBattery::ADCBattery(
   const ADCBatteryParams & params)
 : ReadVoltage(read_voltage), ReadCurrent(read_current), ReadTemp(read_temp), ReadCharge(read_charge)
 {
-  voltage_ma_ = std::make_unique<panther_utils::MovingAverage<float>>(
+  voltage_ma_ = std::make_unique<husarion_ugv_utils::MovingAverage<float>>(
     params.voltage_window_len, std::numeric_limits<float>::quiet_NaN());
-  current_ma_ = std::make_unique<panther_utils::MovingAverage<float>>(
+  current_ma_ = std::make_unique<husarion_ugv_utils::MovingAverage<float>>(
     params.current_window_len, std::numeric_limits<float>::quiet_NaN());
-  temp_ma_ = std::make_unique<panther_utils::MovingAverage<float>>(
+  temp_ma_ = std::make_unique<husarion_ugv_utils::MovingAverage<float>>(
     params.temp_window_len, std::numeric_limits<float>::quiet_NaN());
-  charge_ma_ = std::make_unique<panther_utils::MovingAverage<float>>(
+  charge_ma_ = std::make_unique<husarion_ugv_utils::MovingAverage<float>>(
     params.charge_window_len, std::numeric_limits<float>::quiet_NaN());
 }
 
@@ -197,4 +197,4 @@ std::uint8_t ADCBattery::GetBatteryHealth(const float voltage, const float temp)
   }
 }
 
-}  // namespace panther_battery
+}  // namespace husarion_ugv_battery
diff --git a/panther_battery/src/battery/roboteq_battery.cpp b/husarion_ugv_battery/src/battery/roboteq_battery.cpp
similarity index 94%
rename from panther_battery/src/battery/roboteq_battery.cpp
rename to husarion_ugv_battery/src/battery/roboteq_battery.cpp
index 6cd0f3fd..7d2d870d 100644
--- a/panther_battery/src/battery/roboteq_battery.cpp
+++ b/husarion_ugv_battery/src/battery/roboteq_battery.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_battery/battery/roboteq_battery.hpp"
+#include "husarion_ugv_battery/battery/roboteq_battery.hpp"
 
 #include <cstdint>
 #include <functional>
@@ -23,9 +23,9 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_utils/moving_average.hpp"
+#include "husarion_ugv_utils/moving_average.hpp"
 
-namespace panther_battery
+namespace husarion_ugv_battery
 {
 
 RoboteqBattery::RoboteqBattery(
@@ -33,9 +33,9 @@ RoboteqBattery::RoboteqBattery(
   const RoboteqBatteryParams & params)
 : GetRobotDriverState(get_driver_state), driver_state_timeout_(params.driver_state_timeout)
 {
-  voltage_ma_ = std::make_unique<panther_utils::MovingAverage<float>>(
+  voltage_ma_ = std::make_unique<husarion_ugv_utils::MovingAverage<float>>(
     params.voltage_window_len, std::numeric_limits<float>::quiet_NaN());
-  current_ma_ = std::make_unique<panther_utils::MovingAverage<float>>(
+  current_ma_ = std::make_unique<husarion_ugv_utils::MovingAverage<float>>(
     params.current_window_len, std::numeric_limits<float>::quiet_NaN());
 }
 
@@ -157,4 +157,4 @@ bool RoboteqBattery::DriverStateHeartbeatTimeout()
     [](const DriverStateNamedMsg & driver) { return driver.state.heartbeat_timeout; });
 }
 
-}  // namespace panther_battery
+}  // namespace husarion_ugv_battery
diff --git a/panther_battery/src/battery_driver_node.cpp b/husarion_ugv_battery/src/battery_driver_node.cpp
similarity index 91%
rename from panther_battery/src/battery_driver_node.cpp
rename to husarion_ugv_battery/src/battery_driver_node.cpp
index f3603b87..9ec16665 100644
--- a/panther_battery/src/battery_driver_node.cpp
+++ b/husarion_ugv_battery/src/battery_driver_node.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_battery/battery_driver_node.hpp"
+#include "husarion_ugv_battery/battery_driver_node.hpp"
 
 #include <chrono>
 #include <functional>
@@ -24,15 +24,15 @@
 #include "diagnostic_updater/diagnostic_updater.hpp"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_battery/adc_data_reader.hpp"
-#include "panther_battery/battery/adc_battery.hpp"
-#include "panther_battery/battery/battery.hpp"
-#include "panther_battery/battery/roboteq_battery.hpp"
-#include "panther_battery/battery_publisher/battery_publisher.hpp"
-#include "panther_battery/battery_publisher/dual_battery_publisher.hpp"
-#include "panther_battery/battery_publisher/single_battery_publisher.hpp"
+#include "husarion_ugv_battery/adc_data_reader.hpp"
+#include "husarion_ugv_battery/battery/adc_battery.hpp"
+#include "husarion_ugv_battery/battery/battery.hpp"
+#include "husarion_ugv_battery/battery/roboteq_battery.hpp"
+#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp"
+#include "husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp"
+#include "husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp"
 
-namespace panther_battery
+namespace husarion_ugv_battery
 {
 
 BatteryDriverNode::BatteryDriverNode(
@@ -157,4 +157,4 @@ void BatteryDriverNode::BatteryPubTimerCB()
   battery_publisher_->Publish();
 }
 
-}  // namespace panther_battery
+}  // namespace husarion_ugv_battery
diff --git a/panther_battery/src/battery_publisher/battery_publisher.cpp b/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp
similarity index 96%
rename from panther_battery/src/battery_publisher/battery_publisher.cpp
rename to husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp
index e3ccb14b..e37fcbfe 100644
--- a/panther_battery/src/battery_publisher/battery_publisher.cpp
+++ b/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_battery/battery_publisher/battery_publisher.hpp"
+#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp"
 
 #include <cstdint>
 #include <memory>
@@ -21,7 +21,7 @@
 #include "diagnostic_updater/diagnostic_updater.hpp"
 #include "rclcpp/rclcpp.hpp"
 
-namespace panther_battery
+namespace husarion_ugv_battery
 {
 
 BatteryPublisher::BatteryPublisher(
@@ -140,4 +140,4 @@ rclcpp::Clock::SharedPtr BatteryPublisher::GetClock()
   return std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
 }
 
-}  // namespace panther_battery
+}  // namespace husarion_ugv_battery
diff --git a/panther_battery/src/battery_publisher/dual_battery_publisher.cpp b/husarion_ugv_battery/src/battery_publisher/dual_battery_publisher.cpp
similarity index 96%
rename from panther_battery/src/battery_publisher/dual_battery_publisher.cpp
rename to husarion_ugv_battery/src/battery_publisher/dual_battery_publisher.cpp
index fba8198d..f474b1fe 100644
--- a/panther_battery/src/battery_publisher/dual_battery_publisher.cpp
+++ b/husarion_ugv_battery/src/battery_publisher/dual_battery_publisher.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_battery/battery_publisher/dual_battery_publisher.hpp"
+#include "husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp"
 
 #include <cstdint>
 #include <memory>
@@ -24,11 +24,11 @@
 
 #include "sensor_msgs/msg/battery_state.hpp"
 
-#include "panther_battery/battery/battery.hpp"
-#include "panther_battery/battery_publisher/battery_publisher.hpp"
-#include "panther_utils/ros_utils.hpp"
+#include "husarion_ugv_battery/battery/battery.hpp"
+#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp"
+#include "husarion_ugv_utils/ros_utils.hpp"
 
-namespace panther_battery
+namespace husarion_ugv_battery
 {
 
 DualBatteryPublisher::DualBatteryPublisher(
@@ -172,10 +172,10 @@ ChargingStatusMsg DualBatteryPublisher::MergeChargingStatusMsgs(
   ChargingStatusMsg charging_status_msg;
 
   try {
-    panther_utils::ros::VerifyTimestampGap(
+    husarion_ugv_utils::ros::VerifyTimestampGap(
       charging_status_msg_1.header, charging_status_msg_2.header, std::chrono::seconds(1));
 
-    charging_status_msg.header = panther_utils::ros::MergeHeaders(
+    charging_status_msg.header = husarion_ugv_utils::ros::MergeHeaders(
       charging_status_msg_1.header, charging_status_msg_2.header);
   } catch (const std::exception & e) {
     RCLCPP_ERROR_STREAM_THROTTLE(
@@ -251,4 +251,4 @@ void DualBatteryPublisher::DiagnoseStatus(diagnostic_updater::DiagnosticStatusWr
   status.summary(diagnostic_updater::DiagnosticStatusWrapper::OK, "Battery status monitoring");
 }
 
-}  // namespace panther_battery
+}  // namespace husarion_ugv_battery
diff --git a/panther_battery/src/battery_publisher/single_battery_publisher.cpp b/husarion_ugv_battery/src/battery_publisher/single_battery_publisher.cpp
similarity index 92%
rename from panther_battery/src/battery_publisher/single_battery_publisher.cpp
rename to husarion_ugv_battery/src/battery_publisher/single_battery_publisher.cpp
index 4916e647..92e4d003 100644
--- a/panther_battery/src/battery_publisher/single_battery_publisher.cpp
+++ b/husarion_ugv_battery/src/battery_publisher/single_battery_publisher.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_battery/battery_publisher/single_battery_publisher.hpp"
+#include "husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp"
 
 #include <memory>
 #include <stdexcept>
@@ -23,10 +23,10 @@
 
 #include "sensor_msgs/msg/battery_state.hpp"
 
-#include "panther_battery/battery/battery.hpp"
-#include "panther_battery/battery_publisher/battery_publisher.hpp"
+#include "husarion_ugv_battery/battery/battery.hpp"
+#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp"
 
-namespace panther_battery
+namespace husarion_ugv_battery
 {
 
 SingleBatteryPublisher::SingleBatteryPublisher(
@@ -106,4 +106,4 @@ void SingleBatteryPublisher::DiagnoseStatus(diagnostic_updater::DiagnosticStatus
   status.summary(diagnostic_updater::DiagnosticStatusWrapper::OK, "Battery status monitoring");
 }
 
-}  // namespace panther_battery
+}  // namespace husarion_ugv_battery
diff --git a/panther_battery/src/main.cpp b/husarion_ugv_battery/src/main.cpp
similarity index 86%
rename from panther_battery/src/main.cpp
rename to husarion_ugv_battery/src/main.cpp
index b759fd33..9dc84ddf 100644
--- a/panther_battery/src/main.cpp
+++ b/husarion_ugv_battery/src/main.cpp
@@ -18,13 +18,14 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_battery/battery_driver_node.hpp"
+#include "husarion_ugv_battery/battery_driver_node.hpp"
 
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
 
-  auto battery_driver_node = std::make_shared<panther_battery::BatteryDriverNode>("battery_driver");
+  auto battery_driver_node =
+    std::make_shared<husarion_ugv_battery::BatteryDriverNode>("battery_driver");
 
   try {
     rclcpp::spin(battery_driver_node);
diff --git a/panther_battery/test/battery/test_adc_battery.cpp b/husarion_ugv_battery/test/battery/test_adc_battery.cpp
similarity index 93%
rename from panther_battery/test/battery/test_adc_battery.cpp
rename to husarion_ugv_battery/test/battery/test_adc_battery.cpp
index 986f7cdb..9486c85e 100644
--- a/panther_battery/test/battery/test_adc_battery.cpp
+++ b/husarion_ugv_battery/test/battery/test_adc_battery.cpp
@@ -24,8 +24,8 @@
 
 #include "panther_msgs/msg/charging_status.hpp"
 
-#include "panther_battery/battery/adc_battery.hpp"
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_battery/battery/adc_battery.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
 using ChargingStatusMsg = panther_msgs::msg::ChargingStatus;
@@ -54,7 +54,7 @@ class TestADCBattery : public testing::Test
   float battery_temp_raw_;
   float battery_charge_raw_;
 
-  std::unique_ptr<panther_battery::Battery> battery_;
+  std::unique_ptr<husarion_ugv_battery::Battery> battery_;
 
   BatteryStateMsg battery_state_;
   ChargingStatusMsg charging_status_;
@@ -62,8 +62,8 @@ class TestADCBattery : public testing::Test
 
 TestADCBattery::TestADCBattery()
 {
-  panther_battery::ADCBatteryParams params = {10, 10, 10, 10};
-  battery_ = std::make_unique<panther_battery::ADCBattery>(
+  husarion_ugv_battery::ADCBatteryParams params = {10, 10, 10, 10};
+  battery_ = std::make_unique<husarion_ugv_battery::ADCBattery>(
     [&]() { return battery_voltage_raw_; }, [&]() { return battery_current_raw_; },
     [&]() { return battery_temp_raw_; }, [&]() { return battery_charge_raw_; }, params);
 }
@@ -92,8 +92,9 @@ void TestADCBattery::TestDefaultBatteryStateMsg(
   EXPECT_TRUE(std::isnan(battery_state_.capacity));
   EXPECT_FLOAT_EQ(20.0, battery_state_.design_capacity);
   EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_TECHNOLOGY_LION, battery_state_.power_supply_technology);
-  EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector<float>(battery_state_.cell_voltage));
-  EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector<float>(battery_state_.cell_temperature));
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector<float>(battery_state_.cell_voltage));
+  EXPECT_TRUE(
+    husarion_ugv_utils::test_utils::CheckNaNVector<float>(battery_state_.cell_temperature));
   EXPECT_TRUE(battery_state_.present);
   EXPECT_EQ("user_compartment", battery_state_.location);
 
@@ -116,8 +117,9 @@ void TestADCBattery::TestBatteryStateMsg(
   EXPECT_TRUE(std::isnan(battery_state_.capacity));
   EXPECT_FLOAT_EQ(20.0, battery_state_.design_capacity);
   EXPECT_FLOAT_EQ(expected_temp, battery_state_.temperature);
-  EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector<float>(battery_state_.cell_voltage));
-  EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector<float>(battery_state_.cell_temperature));
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector<float>(battery_state_.cell_voltage));
+  EXPECT_TRUE(
+    husarion_ugv_utils::test_utils::CheckNaNVector<float>(battery_state_.cell_temperature));
   EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_TECHNOLOGY_LION, battery_state_.power_supply_technology);
   EXPECT_EQ("user_compartment", battery_state_.location);
 
diff --git a/panther_battery/test/battery/test_battery.cpp b/husarion_ugv_battery/test/battery/test_battery.cpp
similarity index 93%
rename from panther_battery/test/battery/test_battery.cpp
rename to husarion_ugv_battery/test/battery/test_battery.cpp
index cdbdf381..9b2d12c0 100644
--- a/panther_battery/test/battery/test_battery.cpp
+++ b/husarion_ugv_battery/test/battery/test_battery.cpp
@@ -24,13 +24,13 @@
 
 #include "panther_msgs/msg/charging_status.hpp"
 
-#include "panther_battery/battery/battery.hpp"
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_battery/battery/battery.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
 using ChargingStatusMsg = panther_msgs::msg::ChargingStatus;
 
-class BatteryWrapper : public panther_battery::Battery
+class BatteryWrapper : public husarion_ugv_battery::Battery
 {
 public:
   BatteryWrapper() {}
@@ -80,8 +80,9 @@ void TestBattery::TestDefaultBatteryStateMsg(
   EXPECT_TRUE(std::isnan(battery_state_.capacity));
   EXPECT_FLOAT_EQ(20.0, battery_state_.design_capacity);
   EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_TECHNOLOGY_LION, battery_state_.power_supply_technology);
-  EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector<float>(battery_state_.cell_voltage));
-  EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector<float>(battery_state_.cell_temperature));
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector<float>(battery_state_.cell_voltage));
+  EXPECT_TRUE(
+    husarion_ugv_utils::test_utils::CheckNaNVector<float>(battery_state_.cell_temperature));
   EXPECT_TRUE(battery_state_.present);
   EXPECT_EQ("user_compartment", battery_state_.location);
 
diff --git a/panther_battery/test/battery/test_roboteq_battery.cpp b/husarion_ugv_battery/test/battery/test_roboteq_battery.cpp
similarity index 92%
rename from panther_battery/test/battery/test_roboteq_battery.cpp
rename to husarion_ugv_battery/test/battery/test_roboteq_battery.cpp
index 8ca850f1..26674df8 100644
--- a/panther_battery/test/battery/test_roboteq_battery.cpp
+++ b/husarion_ugv_battery/test/battery/test_roboteq_battery.cpp
@@ -27,18 +27,18 @@
 #include "panther_msgs/msg/driver_state_named.hpp"
 #include "panther_msgs/msg/robot_driver_state.hpp"
 
-#include "panther_battery/battery/roboteq_battery.hpp"
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_battery/battery/roboteq_battery.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
 using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
 
-class RoboteqBatteryWrapper : public panther_battery::RoboteqBattery
+class RoboteqBatteryWrapper : public husarion_ugv_battery::RoboteqBattery
 {
 public:
   RoboteqBatteryWrapper(
     const std::function<RobotDriverStateMsg::SharedPtr()> & get_driver_state,
-    const panther_battery::RoboteqBatteryParams & params)
+    const husarion_ugv_battery::RoboteqBatteryParams & params)
   : RoboteqBattery(get_driver_state, params)
   {
   }
@@ -73,7 +73,7 @@ class TestRoboteqBattery : public testing::Test
 
 TestRoboteqBattery::TestRoboteqBattery()
 {
-  const panther_battery::RoboteqBatteryParams params = {kRobotDriverStateTimeout, 10, 10};
+  const husarion_ugv_battery::RoboteqBatteryParams params = {kRobotDriverStateTimeout, 10, 10};
   battery_ = std::make_unique<RoboteqBatteryWrapper>([&]() { return driver_state_; }, params);
 }
 
@@ -105,8 +105,8 @@ void TestRoboteqBattery::TestDefaultBatteryStateMsg(
   EXPECT_TRUE(std::isnan(battery_state_.capacity));
   EXPECT_FLOAT_EQ(20.0, battery_state_.design_capacity);
   EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_TECHNOLOGY_LION, battery_state_.power_supply_technology);
-  EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(battery_state_.cell_voltage));
-  EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(battery_state_.cell_temperature));
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector(battery_state_.cell_voltage));
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector(battery_state_.cell_temperature));
   EXPECT_TRUE(battery_state_.present);
   EXPECT_EQ("user_compartment", battery_state_.location);
 
@@ -128,8 +128,8 @@ void TestRoboteqBattery::TestBatteryStateMsg(
   EXPECT_TRUE(std::isnan(battery_state_.capacity));
   EXPECT_TRUE(std::isnan(battery_state_.temperature));
   EXPECT_FLOAT_EQ(20.0, battery_state_.design_capacity);
-  EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(battery_state_.cell_voltage));
-  EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(battery_state_.cell_temperature));
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector(battery_state_.cell_voltage));
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector(battery_state_.cell_temperature));
   EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_TECHNOLOGY_LION, battery_state_.power_supply_technology);
   EXPECT_EQ("user_compartment", battery_state_.location);
 
diff --git a/panther_battery/test/battery_publisher/test_battery_publisher.cpp b/husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp
similarity index 94%
rename from panther_battery/test/battery_publisher/test_battery_publisher.cpp
rename to husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp
index 4ed567b1..f5924c73 100644
--- a/panther_battery/test/battery_publisher/test_battery_publisher.cpp
+++ b/husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp
@@ -24,18 +24,18 @@
 
 #include "panther_msgs/msg/io_state.hpp"
 
-#include "panther_battery/battery_publisher/battery_publisher.hpp"
+#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp"
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
 using IOStateMsg = panther_msgs::msg::IOState;
 
-class BatteryPublisherWrapper : public panther_battery::BatteryPublisher
+class BatteryPublisherWrapper : public husarion_ugv_battery::BatteryPublisher
 {
 public:
   BatteryPublisherWrapper(
     const rclcpp::Node::SharedPtr & node,
     std::shared_ptr<diagnostic_updater::Updater> diagnostic_updater)
-  : panther_battery::BatteryPublisher(node, diagnostic_updater)
+  : husarion_ugv_battery::BatteryPublisher(node, diagnostic_updater)
   {
   }
 
diff --git a/panther_battery/test/battery_publisher/test_dual_battery_publisher.cpp b/husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp
similarity index 93%
rename from panther_battery/test/battery_publisher/test_dual_battery_publisher.cpp
rename to husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp
index 0740b7fa..74bcb358 100644
--- a/panther_battery/test/battery_publisher/test_dual_battery_publisher.cpp
+++ b/husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp
@@ -24,22 +24,22 @@
 
 #include "panther_msgs/msg/charging_status.hpp"
 
-#include "panther_battery/battery/adc_battery.hpp"
-#include "panther_battery/battery/battery.hpp"
-#include "panther_battery/battery_publisher/dual_battery_publisher.hpp"
-#include "panther_utils/test/ros_test_utils.hpp"
+#include "husarion_ugv_battery/battery/adc_battery.hpp"
+#include "husarion_ugv_battery/battery/battery.hpp"
+#include "husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp"
+#include "husarion_ugv_utils/test/ros_test_utils.hpp"
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
 using ChargingStatusMsg = panther_msgs::msg::ChargingStatus;
 
-class DualBatteryPublisherWrapper : public panther_battery::DualBatteryPublisher
+class DualBatteryPublisherWrapper : public husarion_ugv_battery::DualBatteryPublisher
 {
 public:
   DualBatteryPublisherWrapper(
     const rclcpp::Node::SharedPtr & node,
     std::shared_ptr<diagnostic_updater::Updater> diagnostic_updater,
-    std::shared_ptr<panther_battery::Battery> & battery_1,
-    std::shared_ptr<panther_battery::Battery> & battery_2)
+    std::shared_ptr<husarion_ugv_battery::Battery> & battery_1,
+    std::shared_ptr<husarion_ugv_battery::Battery> & battery_2)
   : DualBatteryPublisher(node, diagnostic_updater, battery_1, battery_2)
   {
   }
@@ -90,8 +90,8 @@ class TestDualBatteryPublisher : public testing::Test
   rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_1_sub_;
   rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_2_sub_;
 
-  std::shared_ptr<panther_battery::Battery> battery_1_;
-  std::shared_ptr<panther_battery::Battery> battery_2_;
+  std::shared_ptr<husarion_ugv_battery::Battery> battery_1_;
+  std::shared_ptr<husarion_ugv_battery::Battery> battery_2_;
   std::shared_ptr<DualBatteryPublisherWrapper> battery_publisher_;
   BatteryStateMsg::SharedPtr battery_state_;
   BatteryStateMsg::SharedPtr battery_1_state_;
@@ -100,11 +100,11 @@ class TestDualBatteryPublisher : public testing::Test
 
 TestDualBatteryPublisher::TestDualBatteryPublisher()
 {
-  panther_battery::ADCBatteryParams params = {10, 10, 10, 10};
-  battery_1_ = std::make_shared<panther_battery::ADCBattery>(
+  husarion_ugv_battery::ADCBatteryParams params = {10, 10, 10, 10};
+  battery_1_ = std::make_shared<husarion_ugv_battery::ADCBattery>(
     [&]() { return 1.6; }, [&]() { return 0.02; }, [&]() { return 1.6; }, [&]() { return 0.4; },
     params);
-  battery_2_ = std::make_shared<panther_battery::ADCBattery>(
+  battery_2_ = std::make_shared<husarion_ugv_battery::ADCBattery>(
     [&]() { return 1.6; }, [&]() { return 0.02; }, [&]() { return 1.6; }, [&]() { return 0.4; },
     params);
 
@@ -139,13 +139,13 @@ void TestDualBatteryPublisher::TestMergeBatteryPowerSupplyStatus(
 TEST_F(TestDualBatteryPublisher, CorrectTopicPublished)
 {
   battery_publisher_->Publish();
-  ASSERT_TRUE(
-    panther_utils::test_utils::WaitForMsg(node_, battery_state_, std::chrono::milliseconds(1000)));
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
+    node_, battery_state_, std::chrono::milliseconds(1000)));
   battery_publisher_->Publish();
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     node_, battery_1_state_, std::chrono::milliseconds(1000)));
   battery_publisher_->Publish();
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     node_, battery_2_state_, std::chrono::milliseconds(1000)));
 }
 
diff --git a/panther_battery/test/battery_publisher/test_single_battery_publisher.cpp b/husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp
similarity index 75%
rename from panther_battery/test/battery_publisher/test_single_battery_publisher.cpp
rename to husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp
index f7a04c3f..3d98a9a7 100644
--- a/panther_battery/test/battery_publisher/test_single_battery_publisher.cpp
+++ b/husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp
@@ -21,10 +21,10 @@
 
 #include "sensor_msgs/msg/battery_state.hpp"
 
-#include "panther_battery/battery/adc_battery.hpp"
-#include "panther_battery/battery/battery.hpp"
-#include "panther_battery/battery_publisher/single_battery_publisher.hpp"
-#include "panther_utils/test/ros_test_utils.hpp"
+#include "husarion_ugv_battery/battery/adc_battery.hpp"
+#include "husarion_ugv_battery/battery/battery.hpp"
+#include "husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp"
+#include "husarion_ugv_utils/test/ros_test_utils.hpp"
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
 
@@ -40,16 +40,16 @@ class TestSingleBatteryPublisher : public testing::Test
   rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_sub_;
   rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_1_sub_;
 
-  std::shared_ptr<panther_battery::Battery> battery_;
-  std::shared_ptr<panther_battery::BatteryPublisher> battery_publisher_;
+  std::shared_ptr<husarion_ugv_battery::Battery> battery_;
+  std::shared_ptr<husarion_ugv_battery::BatteryPublisher> battery_publisher_;
   BatteryStateMsg::SharedPtr battery_state_;
   BatteryStateMsg::SharedPtr battery_1_state_;
 };
 
 TestSingleBatteryPublisher::TestSingleBatteryPublisher()
 {
-  panther_battery::ADCBatteryParams params = {10, 10, 10, 10};
-  battery_ = std::make_shared<panther_battery::ADCBattery>(
+  husarion_ugv_battery::ADCBatteryParams params = {10, 10, 10, 10};
+  battery_ = std::make_shared<husarion_ugv_battery::ADCBattery>(
     [&]() { return 1.6; }, [&]() { return 0.02; }, [&]() { return 1.6; }, [&]() { return 0.4; },
     params);
 
@@ -61,17 +61,17 @@ TestSingleBatteryPublisher::TestSingleBatteryPublisher()
   battery_1_sub_ = node_->create_subscription<BatteryStateMsg>(
     "/_battery/battery_1_status_raw", 10,
     [&](const BatteryStateMsg::SharedPtr msg) { battery_1_state_ = msg; });
-  battery_publisher_ = std::make_shared<panther_battery::SingleBatteryPublisher>(
+  battery_publisher_ = std::make_shared<husarion_ugv_battery::SingleBatteryPublisher>(
     node_, diagnostic_updater_, battery_);
 }
 
 TEST_F(TestSingleBatteryPublisher, CorrectTopicPublished)
 {
   battery_publisher_->Publish();
-  ASSERT_TRUE(
-    panther_utils::test_utils::WaitForMsg(node_, battery_state_, std::chrono::milliseconds(1000)));
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
+    node_, battery_state_, std::chrono::milliseconds(1000)));
   battery_publisher_->Publish();
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     node_, battery_1_state_, std::chrono::milliseconds(1000)));
 }
 
diff --git a/panther_battery/test/test_adc_data_reader.cpp b/husarion_ugv_battery/test/test_adc_data_reader.cpp
similarity index 92%
rename from panther_battery/test/test_adc_data_reader.cpp
rename to husarion_ugv_battery/test/test_adc_data_reader.cpp
index c9fd3d94..bbb322bc 100644
--- a/panther_battery/test/test_adc_data_reader.cpp
+++ b/husarion_ugv_battery/test/test_adc_data_reader.cpp
@@ -18,7 +18,7 @@
 
 #include "gtest/gtest.h"
 
-#include "panther_battery/adc_data_reader.hpp"
+#include "husarion_ugv_battery/adc_data_reader.hpp"
 
 class TestADCDataReader : public testing::Test
 {
@@ -27,7 +27,7 @@ class TestADCDataReader : public testing::Test
   ~TestADCDataReader();
 
 protected:
-  std::shared_ptr<panther_battery::ADCDataReader> data_reader_;
+  std::shared_ptr<husarion_ugv_battery::ADCDataReader> data_reader_;
   std::filesystem::path data_file_path_;
   std::filesystem::path scale_file_path_;
   std::ofstream file_;
@@ -44,7 +44,7 @@ TestADCDataReader::TestADCDataReader()
 
   WriteNumberToFile<float>(1.0, scale_file_path_);
 
-  data_reader_ = std::make_shared<panther_battery::ADCDataReader>(current_path);
+  data_reader_ = std::make_shared<husarion_ugv_battery::ADCDataReader>(current_path);
 }
 
 TestADCDataReader::~TestADCDataReader()
diff --git a/panther_battery/test/test_battery_driver_node_adc_dual.cpp b/husarion_ugv_battery/test/test_battery_driver_node_adc_dual.cpp
similarity index 91%
rename from panther_battery/test/test_battery_driver_node_adc_dual.cpp
rename to husarion_ugv_battery/test/test_battery_driver_node_adc_dual.cpp
index 740ba838..2718407a 100644
--- a/panther_battery/test/test_battery_driver_node_adc_dual.cpp
+++ b/husarion_ugv_battery/test/test_battery_driver_node_adc_dual.cpp
@@ -20,7 +20,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_utils/test/ros_test_utils.hpp"
+#include "husarion_ugv_utils/test/ros_test_utils.hpp"
 
 class TestBatteryNodeADCDual : public TestBatteryNode
 {
@@ -30,7 +30,7 @@ class TestBatteryNodeADCDual : public TestBatteryNode
 
 TEST_F(TestBatteryNodeADCDual, BatteryValues)
 {
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(5000)));
 
   // This is done to check if channels of ADC readers were assigned correctly, not to verify
@@ -51,7 +51,7 @@ TEST_F(TestBatteryNodeADCDual, BatteryValues)
   // Change value of battery 2 reading one by one and check if corresponding values in battery 1
   // stops matching
   WriteNumberToFile<int>(1600, std::filesystem::path(device1_path_ / "in_voltage3_raw"));
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_2_state_, std::chrono::milliseconds(1000)));
   EXPECT_FALSE(
     fabs(battery_1_state_->voltage - battery_2_state_->voltage) <
@@ -67,7 +67,7 @@ TEST_F(TestBatteryNodeADCDual, BatteryValues)
 
   WriteNumberToFile<int>(900, std::filesystem::path(device1_path_ / "in_voltage1_raw"));
   WriteNumberToFile<int>(100, std::filesystem::path(device0_path_ / "in_voltage2_raw"));
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_2_state_, std::chrono::milliseconds(1000)));
   EXPECT_FALSE(
     fabs(battery_1_state_->current - battery_2_state_->current) <
@@ -75,7 +75,7 @@ TEST_F(TestBatteryNodeADCDual, BatteryValues)
   EXPECT_FLOAT_EQ(battery_1_state_->temperature, battery_2_state_->temperature);
 
   WriteNumberToFile<int>(1000, std::filesystem::path(device0_path_ / "in_voltage0_raw"));
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_2_state_, std::chrono::milliseconds(1000)));
   EXPECT_FALSE(
     fabs(battery_1_state_->temperature - battery_2_state_->temperature) <
@@ -84,7 +84,7 @@ TEST_F(TestBatteryNodeADCDual, BatteryValues)
 
 TEST_F(TestBatteryNodeADCDual, BatteryTimeout)
 {
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(5000)));
 
   // Battery state msg should have some values
@@ -98,7 +98,7 @@ TEST_F(TestBatteryNodeADCDual, BatteryTimeout)
   std::filesystem::remove(std::filesystem::path(device0_path_ / "in_voltage2_raw"));
   std::filesystem::remove(std::filesystem::path(device1_path_ / "in_voltage2_raw"));
   std::this_thread::sleep_for(std::chrono::milliseconds(2500));
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(1000)));
 
   // Battery state msg values should be NaN
@@ -114,14 +114,14 @@ TEST_F(TestBatteryNodeADCDual, BatteryTimeout)
 TEST_F(TestBatteryNodeADCDual, BatteryCharging)
 {
   // Wait for node to initialize
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(5000)));
 
   // Publish charger connected state
   IOStateMsg io_state;
   io_state.charger_connected = true;
   io_state_pub_->publish(io_state);
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(1000)));
 
   EXPECT_NE(battery_state_->power_supply_status, BatteryStateMsg::POWER_SUPPLY_STATUS_DISCHARGING);
diff --git a/panther_battery/test/test_battery_driver_node_adc_single.cpp b/husarion_ugv_battery/test/test_battery_driver_node_adc_single.cpp
similarity index 91%
rename from panther_battery/test/test_battery_driver_node_adc_single.cpp
rename to husarion_ugv_battery/test/test_battery_driver_node_adc_single.cpp
index cc26bfe7..8c0ec498 100644
--- a/panther_battery/test/test_battery_driver_node_adc_single.cpp
+++ b/husarion_ugv_battery/test/test_battery_driver_node_adc_single.cpp
@@ -20,7 +20,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_utils/test/ros_test_utils.hpp"
+#include "husarion_ugv_utils/test/ros_test_utils.hpp"
 
 class TestBatteryNodeADCSingle : public TestBatteryNode
 {
@@ -35,7 +35,7 @@ TEST_F(TestBatteryNodeADCSingle, BatteryValues)
   WriteNumberToFile<int>(1600, std::filesystem::path(device1_path_ / "in_voltage3_raw"));
   WriteNumberToFile<int>(100, std::filesystem::path(device0_path_ / "in_voltage2_raw"));
 
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(5000)));
 
   // This is done to check if channels of ADC readers were assigned correctly, not to verify
@@ -56,7 +56,7 @@ TEST_F(TestBatteryNodeADCSingle, BatteryValues)
 
 TEST_F(TestBatteryNodeADCSingle, BatteryTimeout)
 {
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(5000)));
 
   // Battery state msg should have some values
@@ -70,7 +70,7 @@ TEST_F(TestBatteryNodeADCSingle, BatteryTimeout)
   std::filesystem::remove(std::filesystem::path(device0_path_ / "in_voltage2_raw"));
   std::filesystem::remove(std::filesystem::path(device1_path_ / "in_voltage2_raw"));
   std::this_thread::sleep_for(std::chrono::milliseconds(2500));
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(1000)));
 
   // Battery state msg values should be NaN
@@ -86,14 +86,14 @@ TEST_F(TestBatteryNodeADCSingle, BatteryTimeout)
 TEST_F(TestBatteryNodeADCSingle, BatteryCharging)
 {
   // Wait for node to initialize
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(5000)));
 
   // Publish charger connected state
   IOStateMsg io_state;
   io_state.charger_connected = true;
   io_state_pub_->publish(io_state);
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(1000)));
 
   EXPECT_NE(battery_state_->power_supply_status, BatteryStateMsg::POWER_SUPPLY_STATUS_DISCHARGING);
@@ -105,7 +105,7 @@ TEST_F(TestBatteryNodeADCSingle, RoboteqInitOnADCFail)
   std::filesystem::remove_all(device0_path_);
 
   // Wait for node to initialize
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(5000)));
 
   // Battery state status should be UNKNOWN
@@ -116,7 +116,7 @@ TEST_F(TestBatteryNodeADCSingle, RoboteqInitOnADCFail)
   driver_state.header.stamp = battery_driver_node_->get_clock()->now();
   driver_state_pub_->publish(driver_state);
 
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(1000)));
 
   // Battery state status should be different than UNKNOWN
diff --git a/panther_battery/test/test_battery_driver_node_roboteq.cpp b/husarion_ugv_battery/test/test_battery_driver_node_roboteq.cpp
similarity index 93%
rename from panther_battery/test/test_battery_driver_node_roboteq.cpp
rename to husarion_ugv_battery/test/test_battery_driver_node_roboteq.cpp
index 84eb00b9..8393b1b9 100644
--- a/panther_battery/test/test_battery_driver_node_roboteq.cpp
+++ b/husarion_ugv_battery/test/test_battery_driver_node_roboteq.cpp
@@ -22,7 +22,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_utils/test/ros_test_utils.hpp"
+#include "husarion_ugv_utils/test/ros_test_utils.hpp"
 
 class TestBatteryNodeRoboteq : public TestBatteryNode
 {
@@ -33,7 +33,7 @@ class TestBatteryNodeRoboteq : public TestBatteryNode
 TEST_F(TestBatteryNodeRoboteq, BatteryValues)
 {
   // Wait for node to initialize
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(5000)));
 
   // Battery state msg values should be NaN
@@ -54,7 +54,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryValues)
   driver_state.driver_states.push_back(motor_controller);
   driver_state_pub_->publish(driver_state);
 
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(1000)));
 
   // This is done to check if values were read correctly, not to verify calculations.
@@ -72,7 +72,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryValues)
 TEST_F(TestBatteryNodeRoboteq, BatteryTimeout)
 {
   // Wait for node to initialize
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(5000)));
 
   // Battery state msg values should be NaN
@@ -94,7 +94,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout)
   driver_state.driver_states.push_back(motor_controller);
   driver_state_pub_->publish(driver_state);
 
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(1000)));
 
   // Battery state msg should have some values
@@ -105,7 +105,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout)
 
   // Wait for timeout
   std::this_thread::sleep_for(std::chrono::milliseconds(1500));
-  ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(
+  ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg(
     battery_driver_node_, battery_state_, std::chrono::milliseconds(1000)));
 
   // Battery state msg values should be NaN
diff --git a/panther_battery/test/utils/test_battery_driver_node.hpp b/husarion_ugv_battery/test/utils/test_battery_driver_node.hpp
similarity index 93%
rename from panther_battery/test/utils/test_battery_driver_node.hpp
rename to husarion_ugv_battery/test/utils/test_battery_driver_node.hpp
index bad9542c..319dfd6d 100644
--- a/panther_battery/test/utils/test_battery_driver_node.hpp
+++ b/husarion_ugv_battery/test/utils/test_battery_driver_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_
-#define PANTHER_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_
+#ifndef HUSARION_UGV_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_
+#define HUSARION_UGV_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_
 
 #include <filesystem>
 #include <fstream>
@@ -31,7 +31,7 @@
 #include "panther_msgs/msg/io_state.hpp"
 #include "panther_msgs/msg/robot_driver_state.hpp"
 
-#include "panther_battery/battery_driver_node.hpp"
+#include "husarion_ugv_battery/battery_driver_node.hpp"
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
 using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
@@ -55,7 +55,7 @@ class TestBatteryNode : public testing::Test
   BatteryStateMsg::SharedPtr battery_state_;
   BatteryStateMsg::SharedPtr battery_1_state_;
   BatteryStateMsg::SharedPtr battery_2_state_;
-  std::shared_ptr<panther_battery::BatteryDriverNode> battery_driver_node_;
+  std::shared_ptr<husarion_ugv_battery::BatteryDriverNode> battery_driver_node_;
   rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_sub_;
   rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_1_sub_;
   rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_2_sub_;
@@ -103,7 +103,7 @@ TestBatteryNode::TestBatteryNode(const bool use_adc_battery, const bool dual_bat
   rclcpp::NodeOptions options;
   options.parameter_overrides(params);
 
-  battery_driver_node_ = std::make_shared<panther_battery::BatteryDriverNode>(
+  battery_driver_node_ = std::make_shared<husarion_ugv_battery::BatteryDriverNode>(
     "battery_driver", options);
 
   battery_sub_ = battery_driver_node_->create_subscription<BatteryStateMsg>(
@@ -145,4 +145,4 @@ void TestBatteryNode::WriteNumberToFile(const T number, const std::string & file
   }
 }
 
-#endif  // PANTHER_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_
+#endif  // HUSARION_UGV_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_
diff --git a/panther_bringup/CHANGELOG.rst b/husarion_ugv_bringup/CHANGELOG.rst
similarity index 100%
rename from panther_bringup/CHANGELOG.rst
rename to husarion_ugv_bringup/CHANGELOG.rst
diff --git a/panther_bringup/CMakeLists.txt b/husarion_ugv_bringup/CMakeLists.txt
similarity index 83%
rename from panther_bringup/CMakeLists.txt
rename to husarion_ugv_bringup/CMakeLists.txt
index 79305750..6fb4694d 100644
--- a/panther_bringup/CMakeLists.txt
+++ b/husarion_ugv_bringup/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10.2)
-project(panther_bringup)
+project(husarion_ugv_bringup)
 
 find_package(ament_cmake REQUIRED)
 
diff --git a/panther_bringup/README.md b/husarion_ugv_bringup/README.md
similarity index 87%
rename from panther_bringup/README.md
rename to husarion_ugv_bringup/README.md
index b12b4218..3615bce3 100644
--- a/panther_bringup/README.md
+++ b/husarion_ugv_bringup/README.md
@@ -1,6 +1,6 @@
-# panther_bringup
+# husarion_ugv_bringup
 
-The package contains the default configuration and launch files necessary to start all the basic functionalities of the Husarion Panther robot.
+The package contains the default configuration and launch files necessary to start all the basic functionalities of the Husarion UGV.
 
 ## Launch Files
 
diff --git a/panther_bringup/launch/bringup.launch.py b/husarion_ugv_bringup/launch/bringup.launch.py
similarity index 87%
rename from panther_bringup/launch/bringup.launch.py
rename to husarion_ugv_bringup/launch/bringup.launch.py
index 6aaa91ef..78e52927 100644
--- a/panther_bringup/launch/bringup.launch.py
+++ b/husarion_ugv_bringup/launch/bringup.launch.py
@@ -16,6 +16,7 @@
 
 import os
 
+from husarion_ugv_utils.messages import welcome_msg
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction
 from launch.conditions import UnlessCondition
@@ -26,7 +27,6 @@
     PathJoinSubstitution,
 )
 from launch_ros.substitutions import FindPackageShare
-from panther_utils.messages import welcome_msg
 
 
 def generate_launch_description():
@@ -55,7 +55,7 @@ def generate_launch_description():
     controller_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare("panther_controller"), "launch", "controller.launch.py"]
+                [FindPackageShare("husarion_ugv_controller"), "launch", "controller.launch.py"]
             )
         ),
         launch_arguments={"namespace": namespace}.items(),
@@ -65,7 +65,7 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
                 [
-                    FindPackageShare("panther_diagnostics"),
+                    FindPackageShare("husarion_ugv_diagnostics"),
                     "launch",
                     "system_monitor.launch.py",
                 ]
@@ -77,7 +77,7 @@ def generate_launch_description():
     lights_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare("panther_lights"), "launch", "lights.launch.py"]
+                [FindPackageShare("husarion_ugv_lights"), "launch", "lights.launch.py"]
             )
         ),
         launch_arguments={"namespace": namespace}.items(),
@@ -86,7 +86,7 @@ def generate_launch_description():
     battery_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare("panther_battery"), "launch", "battery.launch.py"]
+                [FindPackageShare("husarion_ugv_battery"), "launch", "battery.launch.py"]
             ),
         ),
         launch_arguments={"namespace": namespace}.items(),
@@ -95,7 +95,7 @@ def generate_launch_description():
     ekf_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare("panther_localization"), "launch", "localization.launch.py"]
+                [FindPackageShare("husarion_ugv_localization"), "launch", "localization.launch.py"]
             )
         ),
         launch_arguments={"namespace": namespace}.items(),
@@ -104,7 +104,7 @@ def generate_launch_description():
     manager_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare("panther_manager"), "launch", "manager.launch.py"]
+                [FindPackageShare("husarion_ugv_manager"), "launch", "manager.launch.py"]
             )
         ),
         condition=UnlessCondition(disable_manager),
diff --git a/panther_bringup/package.xml b/husarion_ugv_bringup/package.xml
similarity index 81%
rename from panther_bringup/package.xml
rename to husarion_ugv_bringup/package.xml
index a5fe44b4..c2bcda57 100644
--- a/panther_bringup/package.xml
+++ b/husarion_ugv_bringup/package.xml
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>panther_bringup</name>
+  <name>husarion_ugv_bringup</name>
   <version>2.1.1</version>
-  <description>Default launch files and configuration used to start Husarion Panther robot</description>
+  <description>Default launch files and configuration used to start Husarion UGV</description>
   <maintainer email="support@husarion.com">Husarion</maintainer>
   <license>Apache License 2.0</license>
 
@@ -15,15 +15,15 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">husarion_ugv_battery</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">husarion_ugv_controller</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">husarion_ugv_diagnostics</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">husarion_ugv_lights</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">husarion_ugv_localization</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">husarion_ugv_manager</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">husarion_ugv_utils</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">launch</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">launch_ros</exec_depend>
-  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">panther_battery</exec_depend>
-  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">panther_controller</exec_depend>
-  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">panther_diagnostics</exec_depend>
-  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">panther_lights</exec_depend>
-  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">panther_localization</exec_depend>
-  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">panther_manager</exec_depend>
-  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">panther_utils</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/panther_controller/CHANGELOG.rst b/husarion_ugv_controller/CHANGELOG.rst
similarity index 100%
rename from panther_controller/CHANGELOG.rst
rename to husarion_ugv_controller/CHANGELOG.rst
diff --git a/panther_localization/CMakeLists.txt b/husarion_ugv_controller/CMakeLists.txt
similarity index 82%
rename from panther_localization/CMakeLists.txt
rename to husarion_ugv_controller/CMakeLists.txt
index 33da28a6..9cf8423a 100644
--- a/panther_localization/CMakeLists.txt
+++ b/husarion_ugv_controller/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10.2)
-project(panther_localization)
+project(husarion_ugv_controller)
 
 find_package(ament_cmake REQUIRED)
 
diff --git a/husarion_ugv_controller/CONFIGURATION.md b/husarion_ugv_controller/CONFIGURATION.md
new file mode 100644
index 00000000..01a40d15
--- /dev/null
+++ b/husarion_ugv_controller/CONFIGURATION.md
@@ -0,0 +1,9 @@
+# husarion_ugv_controller
+
+## Changing Velocity Smoothing Parameters
+
+The default drive controller is based on [diff_drive_controller](https://control.ros.org/master/doc/ros2_controllers/diff_drive_controller/doc/userdoc.html) from [ros2 control](https://control.ros.org/master/index.html) or [mecanum_drive_controller](https://github.com/husarion/husarion_controllers/tree/main/mecanum_drive_controller). This controller can be customized, among others: by modifying the robot's dynamic parameters (e.g. smooth the speed or limit the robot's speed and acceleration). Its parameters can be found in the [husarion_ugv_controller](https://github.com/husarion/panther_ros/tree/ros2-devel/husarion_ugv_controller/config). By default, these values correspond to the upper limits of the robot's velocities and accelerations.
+
+## Changing Wheel Type
+
+Changing wheel types is possible and can be done for both the real robot and the simulation. By default, three types of wheels are supported using the launch argument `wheel_type`. If you want to use custom wheels, all you need to do is point to the new wheel and controller configuration files using the `wheel_config_path` and `controller_config_path` parameters. These files should be based on the default files, i.e. [WH01_controller.yaml](./config/WH01_controller.yaml) and [WH01.yaml](../panther_description/config/WH01.yaml).
diff --git a/panther_controller/README.md b/husarion_ugv_controller/README.md
similarity index 96%
rename from panther_controller/README.md
rename to husarion_ugv_controller/README.md
index ac600dc7..509a5e4e 100644
--- a/panther_controller/README.md
+++ b/husarion_ugv_controller/README.md
@@ -1,6 +1,6 @@
-# panther_controller
+# husarion_ugv_controller
 
-The package contains the default configuration and launch files necessary to start all the basic functionalities of the Husarion Panther robot.
+The package contains the default configuration and launch files necessary to start all the basic functionalities of the Husarion UGV.
 
 ## Launch Files
 
diff --git a/panther_controller/config/WH01_controller.yaml b/husarion_ugv_controller/config/WH01_controller.yaml
similarity index 100%
rename from panther_controller/config/WH01_controller.yaml
rename to husarion_ugv_controller/config/WH01_controller.yaml
diff --git a/panther_controller/config/WH02_controller.yaml b/husarion_ugv_controller/config/WH02_controller.yaml
similarity index 100%
rename from panther_controller/config/WH02_controller.yaml
rename to husarion_ugv_controller/config/WH02_controller.yaml
diff --git a/panther_controller/config/WH04_controller.yaml b/husarion_ugv_controller/config/WH04_controller.yaml
similarity index 98%
rename from panther_controller/config/WH04_controller.yaml
rename to husarion_ugv_controller/config/WH04_controller.yaml
index 25987350..b13bbd11 100644
--- a/panther_controller/config/WH04_controller.yaml
+++ b/husarion_ugv_controller/config/WH04_controller.yaml
@@ -32,7 +32,7 @@
       wheel_separation: 0.616
       wheel_radius: 0.1016
 
-      # todo: check it for panther
+      # TODO: check it
       # For skid drive kinematics it will act as ICR coefficient, kinematic model with ICR
       # coefficient isn't totally accurate and this coefficient can differ for various ground types
       wheel_separation_multiplier: 1.0
diff --git a/panther_controller/config/WH05_controller.yaml b/husarion_ugv_controller/config/WH05_controller.yaml
similarity index 100%
rename from panther_controller/config/WH05_controller.yaml
rename to husarion_ugv_controller/config/WH05_controller.yaml
diff --git a/panther_controller/launch/controller.launch.py b/husarion_ugv_controller/launch/controller.launch.py
similarity index 97%
rename from panther_controller/launch/controller.launch.py
rename to husarion_ugv_controller/launch/controller.launch.py
index adf83965..2e61a370 100644
--- a/panther_controller/launch/controller.launch.py
+++ b/husarion_ugv_controller/launch/controller.launch.py
@@ -52,8 +52,8 @@ def generate_launch_description():
         ),
         description=(
             "Additional components configuration file. Components described in this file "
-            "are dynamically included in Panther's urdf."
-            "Panther options are described here "
+            "are dynamically included in robot's URDF."
+            "Available options are described in the manual: "
             "https://husarion.com/manuals/panther/panther-options/"
         ),
     )
@@ -64,14 +64,14 @@ def generate_launch_description():
         "controller_config_path",
         default_value=PathJoinSubstitution(
             [
-                FindPackageShare("panther_controller"),
+                FindPackageShare("husarion_ugv_controller"),
                 "config",
                 PythonExpression(["'", wheel_type, "_controller.yaml'"]),
             ]
         ),
         description=(
             "Path to controller configuration file. By default, it is located in"
-            " 'panther_controller/config/{wheel_type}_controller.yaml'. You can also specify"
+            " 'husarion_ugv_controller/config/{wheel_type}_controller.yaml'. You can also specify"
             " the path to your custom controller configuration file here. "
         ),
     )
diff --git a/panther_controller/package.xml b/husarion_ugv_controller/package.xml
similarity index 89%
rename from panther_controller/package.xml
rename to husarion_ugv_controller/package.xml
index 6dea3a07..606766aa 100644
--- a/panther_controller/package.xml
+++ b/husarion_ugv_controller/package.xml
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>panther_controller</name>
+  <name>husarion_ugv_controller</name>
   <version>2.1.1</version>
-  <description>ros2 controllers configuration for Panther</description>
+  <description>ros2 controllers configuration for Husarion UGV</description>
   <maintainer email="support@husarion.com">Husarion</maintainer>
   <license>Apache License 2.0</license>
 
@@ -18,6 +18,7 @@
 
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>diff_drive_controller</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">husarion_ugv_hardware_interfaces</exec_depend>
   <exec_depend>imu_sensor_broadcaster</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>launch</exec_depend>
@@ -25,7 +26,6 @@
   <exec_depend>lynx_description</exec_depend>
   <exec_depend>mecanum_drive_controller</exec_depend>
   <exec_depend>panther_description</exec_depend>
-  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">panther_hardware_interfaces</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>xacro</exec_depend>
 
diff --git a/panther_diagnostics/CHANGELOG.rst b/husarion_ugv_diagnostics/CHANGELOG.rst
similarity index 100%
rename from panther_diagnostics/CHANGELOG.rst
rename to husarion_ugv_diagnostics/CHANGELOG.rst
diff --git a/panther_diagnostics/CMakeLists.txt b/husarion_ugv_diagnostics/CMakeLists.txt
similarity index 97%
rename from panther_diagnostics/CMakeLists.txt
rename to husarion_ugv_diagnostics/CMakeLists.txt
index 8794efbe..64b9751d 100644
--- a/panther_diagnostics/CMakeLists.txt
+++ b/husarion_ugv_diagnostics/CMakeLists.txt
@@ -8,7 +8,7 @@ if(USE_SUPERBUILD)
   include(cmake/SuperBuild.cmake)
   return()
 else()
-  project(panther_diagnostics)
+  project(husarion_ugv_diagnostics)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -21,7 +21,7 @@ set(PACKAGE_DEPENDENCIES
     diagnostic_updater
     generate_parameter_library
     panther_msgs
-    panther_utils
+    husarion_ugv_utils
     PkgConfig
     rclcpp
     std_msgs)
diff --git a/panther_diagnostics/README.md b/husarion_ugv_diagnostics/README.md
similarity index 95%
rename from panther_diagnostics/README.md
rename to husarion_ugv_diagnostics/README.md
index d3e213bb..02d37021 100644
--- a/panther_diagnostics/README.md
+++ b/husarion_ugv_diagnostics/README.md
@@ -1,6 +1,6 @@
-# panther_diagnostics
+# husarion_ugv_diagnostics
 
-Package containing nodes monitoring and publishing the Built-in Computer status of Husarion Panther robot.
+Package containing nodes monitoring and publishing the Built-in Computer status of Husarion UGV.
 
 ## Launch Files
 
diff --git a/panther_diagnostics/cmake/SuperBuild.cmake b/husarion_ugv_diagnostics/cmake/SuperBuild.cmake
similarity index 97%
rename from panther_diagnostics/cmake/SuperBuild.cmake
rename to husarion_ugv_diagnostics/cmake/SuperBuild.cmake
index 4370b4cd..83fdadbb 100644
--- a/panther_diagnostics/cmake/SuperBuild.cmake
+++ b/husarion_ugv_diagnostics/cmake/SuperBuild.cmake
@@ -30,7 +30,7 @@ ExternalProject_Add(
   BUILD_IN_SOURCE 1)
 
 ExternalProject_Add(
-  ep_panther_diagnostics
+  ep_husarion_ugv_diagnostics
   DEPENDS ${DEPENDENCIES}
   SOURCE_DIR ${PROJECT_SOURCE_DIR}
   CMAKE_ARGS -DUSE_SUPERBUILD=OFF
diff --git a/panther_diagnostics/config/system_monitor.yaml b/husarion_ugv_diagnostics/config/system_monitor.yaml
similarity index 100%
rename from panther_diagnostics/config/system_monitor.yaml
rename to husarion_ugv_diagnostics/config/system_monitor.yaml
diff --git a/panther_diagnostics/include/panther_diagnostics/filesystem.hpp b/husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/filesystem.hpp
similarity index 93%
rename from panther_diagnostics/include/panther_diagnostics/filesystem.hpp
rename to husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/filesystem.hpp
index fa2291d6..17825312 100644
--- a/panther_diagnostics/include/panther_diagnostics/filesystem.hpp
+++ b/husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/filesystem.hpp
@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_DIAGNOSTICS_FILESYSTEM_HPP_
-#define PANTHER_DIAGNOSTICS_FILESYSTEM_HPP_
+#ifndef HUSARION_UGV_DIAGNOSTICS_FILESYSTEM_HPP_
+#define HUSARION_UGV_DIAGNOSTICS_FILESYSTEM_HPP_
 
 #include <filesystem>
 #include <fstream>
 
-namespace panther_diagnostics
+namespace husarion_ugv_diagnostics
 {
 
 /**
@@ -110,6 +110,6 @@ class Filesystem : public FilesystemInterface
   }
 };
 
-}  // namespace panther_diagnostics
+}  // namespace husarion_ugv_diagnostics
 
-#endif  // PANTHER_DIAGNOSTICS_FILESYSTEM_HPP_
+#endif  // HUSARION_UGV_DIAGNOSTICS_FILESYSTEM_HPP_
diff --git a/panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp b/husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/system_monitor_node.hpp
similarity index 87%
rename from panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp
rename to husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/system_monitor_node.hpp
index c840a246..0532822f 100644
--- a/panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp
+++ b/husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/system_monitor_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_
-#define PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_
+#ifndef HUSARION_UGV_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_
+#define HUSARION_UGV_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_
 
 #include <string>
 
@@ -24,12 +24,12 @@
 
 #include "system_monitor_parameters.hpp"
 
-#include "panther_diagnostics/filesystem.hpp"
-#include "panther_diagnostics/types.hpp"
+#include "husarion_ugv_diagnostics/filesystem.hpp"
+#include "husarion_ugv_diagnostics/types.hpp"
 
 using namespace std::chrono_literals;
 
-namespace panther_diagnostics
+namespace husarion_ugv_diagnostics
 {
 
 class SystemMonitorNode : public rclcpp::Node
@@ -81,5 +81,5 @@ class SystemMonitorNode : public rclcpp::Node
   static constexpr char kTemperatureInfoFilename[] = "/sys/class/thermal/thermal_zone0/temp";
   static constexpr char kRootDirectory[] = "/";
 };
-}  // namespace panther_diagnostics
-#endif  // PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_
+}  // namespace husarion_ugv_diagnostics
+#endif  // HUSARION_UGV_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_
diff --git a/panther_diagnostics/include/panther_diagnostics/types.hpp b/husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/types.hpp
similarity index 82%
rename from panther_diagnostics/include/panther_diagnostics/types.hpp
rename to husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/types.hpp
index 1ed2c88c..4108b56d 100644
--- a/panther_diagnostics/include/panther_diagnostics/types.hpp
+++ b/husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/types.hpp
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_DIAGNOSTICS_TYPES_HPP_
-#define PANTHER_DIAGNOSTICS_TYPES_HPP_
+#ifndef HUSARION_UGV_DIAGNOSTICS_TYPES_HPP_
+#define HUSARION_UGV_DIAGNOSTICS_TYPES_HPP_
 
 #include <vector>
 
-namespace panther_diagnostics
+namespace husarion_ugv_diagnostics
 {
 
 /**
@@ -35,6 +35,6 @@ struct SystemStatus
   float disk_usage;
 };
 
-}  // namespace panther_diagnostics
+}  // namespace husarion_ugv_diagnostics
 
-#endif  // PANTHER_DIAGNOSTICS_TYPES_HPP_
+#endif  // HUSARION_UGV_DIAGNOSTICS_TYPES_HPP_
diff --git a/panther_diagnostics/launch/system_monitor.launch.py b/husarion_ugv_diagnostics/launch/system_monitor.launch.py
similarity index 97%
rename from panther_diagnostics/launch/system_monitor.launch.py
rename to husarion_ugv_diagnostics/launch/system_monitor.launch.py
index e2d2ab70..34a82815 100644
--- a/panther_diagnostics/launch/system_monitor.launch.py
+++ b/husarion_ugv_diagnostics/launch/system_monitor.launch.py
@@ -29,7 +29,7 @@ def generate_launch_description():
     )
 
     system_monitor_node = Node(
-        package="panther_diagnostics",
+        package="husarion_ugv_diagnostics",
         executable="system_monitor_node",
         name="system_monitor",
         namespace=namespace,
diff --git a/panther_diagnostics/package.xml b/husarion_ugv_diagnostics/package.xml
similarity index 87%
rename from panther_diagnostics/package.xml
rename to husarion_ugv_diagnostics/package.xml
index 9f827b6d..fe6258e0 100644
--- a/panther_diagnostics/package.xml
+++ b/husarion_ugv_diagnostics/package.xml
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>panther_diagnostics</name>
+  <name>husarion_ugv_diagnostics</name>
   <version>2.1.1</version>
-  <description>Package for diagnosting usage of OS on the Panther Robot</description>
+  <description>Package for diagnosting usage of OS on the Husarion UGV robot</description>
   <maintainer email="support@husarion.com">Husarion</maintainer>
   <license>Apache License 2.0</license>
 
@@ -20,8 +20,8 @@
   <depend>diagnostic_msgs</depend>
   <depend>diagnostic_updater</depend>
   <depend>generate_parameter_library</depend>
+  <depend>husarion_ugv_utils</depend>
   <depend>panther_msgs</depend>
-  <depend>panther_utils</depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
 
diff --git a/panther_diagnostics/src/main.cpp b/husarion_ugv_diagnostics/src/main.cpp
similarity index 80%
rename from panther_diagnostics/src/main.cpp
rename to husarion_ugv_diagnostics/src/main.cpp
index 8d09517f..61cb38cc 100644
--- a/panther_diagnostics/src/main.cpp
+++ b/husarion_ugv_diagnostics/src/main.cpp
@@ -18,15 +18,15 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include "panther_diagnostics/filesystem.hpp"
-#include "panther_diagnostics/system_monitor_node.hpp"
+#include "husarion_ugv_diagnostics/filesystem.hpp"
+#include "husarion_ugv_diagnostics/system_monitor_node.hpp"
 
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
 
-  auto filesystem = std::make_shared<panther_diagnostics::Filesystem>();
-  auto system_monitor_node = std::make_shared<panther_diagnostics::SystemMonitorNode>(
+  auto filesystem = std::make_shared<husarion_ugv_diagnostics::Filesystem>();
+  auto system_monitor_node = std::make_shared<husarion_ugv_diagnostics::SystemMonitorNode>(
     "system_monitor", filesystem);
 
   try {
diff --git a/panther_diagnostics/src/system_monitor_node.cpp b/husarion_ugv_diagnostics/src/system_monitor_node.cpp
similarity index 89%
rename from panther_diagnostics/src/system_monitor_node.cpp
rename to husarion_ugv_diagnostics/src/system_monitor_node.cpp
index bc921a75..f25bcbe3 100644
--- a/panther_diagnostics/src/system_monitor_node.cpp
+++ b/husarion_ugv_diagnostics/src/system_monitor_node.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_diagnostics/system_monitor_node.hpp"
+#include "husarion_ugv_diagnostics/system_monitor_node.hpp"
 
 #include <chrono>
 #include <exception>
@@ -24,13 +24,13 @@
 
 #include "panther_msgs/msg/system_status.hpp"
 
-#include "panther_utils/common_utilities.hpp"
-#include "panther_utils/ros_utils.hpp"
+#include "husarion_ugv_utils/common_utilities.hpp"
+#include "husarion_ugv_utils/ros_utils.hpp"
 
-#include "panther_diagnostics/filesystem.hpp"
-#include "panther_diagnostics/types.hpp"
+#include "husarion_ugv_diagnostics/filesystem.hpp"
+#include "husarion_ugv_diagnostics/types.hpp"
 
-namespace panther_diagnostics
+namespace husarion_ugv_diagnostics
 {
 
 SystemMonitorNode::SystemMonitorNode(
@@ -101,7 +101,7 @@ float SystemMonitorNode::GetCPUMeanUsage(const std::vector<float> & usages) cons
   });
 
   auto sum = std::accumulate(usages.begin(), usages.end(), 0.0);
-  auto mean_usage = panther_utils::common_utilities::SetPrecision(sum / usages.size(), 2);
+  auto mean_usage = husarion_ugv_utils::common_utilities::SetPrecision(sum / usages.size(), 2);
 
   return mean_usage;
 }
@@ -112,7 +112,7 @@ float SystemMonitorNode::GetCPUTemperature() const
 
   try {
     const auto temperature_str = filesystem_->ReadFile(kTemperatureInfoFilename);
-    temperature = panther_utils::common_utilities::SetPrecision(
+    temperature = husarion_ugv_utils::common_utilities::SetPrecision(
       std::stof(temperature_str) / 1000.0, 2);
   } catch (const std::exception & e) {
     RCLCPP_ERROR_STREAM(
@@ -127,7 +127,8 @@ float SystemMonitorNode::GetRAMUsage() const
   int total = 0, free = 0, available = 0;
   uprofile::getSystemMemory(total, available, free);
 
-  const auto ram_usage = panther_utils::common_utilities::CountPercentage(total - available, total);
+  const auto ram_usage = husarion_ugv_utils::common_utilities::CountPercentage(
+    total - available, total);
   return ram_usage;
 }
 
@@ -138,7 +139,8 @@ float SystemMonitorNode::GetDiskUsage() const
   try {
     const auto capacity = filesystem_->GetSpaceCapacity(kRootDirectory);
     const auto available = filesystem_->GetSpaceAvailable(kRootDirectory);
-    disk_usage = panther_utils::common_utilities::CountPercentage(capacity - available, capacity);
+    disk_usage = husarion_ugv_utils::common_utilities::CountPercentage(
+      capacity - available, capacity);
   } catch (const std::exception & e) {
     RCLCPP_ERROR_STREAM(
       this->get_logger(), "An exception occurred while reading disk usage: " << e.what());
@@ -196,4 +198,4 @@ void SystemMonitorNode::DiagnoseSystem(diagnostic_updater::DiagnosticStatusWrapp
   status.summary(error_level, message);
 }
 
-}  // namespace panther_diagnostics
+}  // namespace husarion_ugv_diagnostics
diff --git a/panther_diagnostics/test/integration/system_monitor_node.test.py b/husarion_ugv_diagnostics/test/integration/system_monitor_node.test.py
similarity index 97%
rename from panther_diagnostics/test/integration/system_monitor_node.test.py
rename to husarion_ugv_diagnostics/test/integration/system_monitor_node.test.py
index dc895713..53b34bd4 100644
--- a/panther_diagnostics/test/integration/system_monitor_node.test.py
+++ b/husarion_ugv_diagnostics/test/integration/system_monitor_node.test.py
@@ -30,7 +30,7 @@
 def generate_test_description():
 
     system_monitor_node = Node(
-        package="panther_diagnostics",
+        package="husarion_ugv_diagnostics",
         executable="system_monitor_node",
     )
 
diff --git a/panther_diagnostics/test/unit/test_filesystem.cpp b/husarion_ugv_diagnostics/test/unit/test_filesystem.cpp
similarity index 91%
rename from panther_diagnostics/test/unit/test_filesystem.cpp
rename to husarion_ugv_diagnostics/test/unit/test_filesystem.cpp
index 3e9c8755..24f7a654 100644
--- a/panther_diagnostics/test/unit/test_filesystem.cpp
+++ b/husarion_ugv_diagnostics/test/unit/test_filesystem.cpp
@@ -18,7 +18,7 @@
 #include <memory>
 #include <string>
 
-#include "panther_diagnostics/filesystem.hpp"
+#include "husarion_ugv_diagnostics/filesystem.hpp"
 
 class TestFilesystem : public testing::Test
 {
@@ -29,12 +29,13 @@ class TestFilesystem : public testing::Test
   void RemoveTestFile(const std::string & file_path);
 
 protected:
-  std::shared_ptr<panther_diagnostics::Filesystem> filesystem_;
+  std::shared_ptr<husarion_ugv_diagnostics::Filesystem> filesystem_;
 
   static constexpr char kDummyString[] = "Hello World!";
 };
 
-TestFilesystem::TestFilesystem() : filesystem_(std::make_shared<panther_diagnostics::Filesystem>())
+TestFilesystem::TestFilesystem()
+: filesystem_(std::make_shared<husarion_ugv_diagnostics::Filesystem>())
 {
 }
 
diff --git a/panther_diagnostics/test/unit/test_system_monitor_node.cpp b/husarion_ugv_diagnostics/test/unit/test_system_monitor_node.cpp
similarity index 83%
rename from panther_diagnostics/test/unit/test_system_monitor_node.cpp
rename to husarion_ugv_diagnostics/test/unit/test_system_monitor_node.cpp
index 8c93ff24..dd6ad053 100644
--- a/panther_diagnostics/test/unit/test_system_monitor_node.cpp
+++ b/husarion_ugv_diagnostics/test/unit/test_system_monitor_node.cpp
@@ -20,10 +20,10 @@
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
 
-#include "panther_diagnostics/filesystem.hpp"
-#include "panther_diagnostics/system_monitor_node.hpp"
+#include "husarion_ugv_diagnostics/filesystem.hpp"
+#include "husarion_ugv_diagnostics/system_monitor_node.hpp"
 
-class MockFilesystem : public panther_diagnostics::FilesystemInterface
+class MockFilesystem : public husarion_ugv_diagnostics::FilesystemInterface
 {
 public:
   MOCK_METHOD(
@@ -35,38 +35,38 @@ class MockFilesystem : public panther_diagnostics::FilesystemInterface
   MOCK_METHOD(std::string, ReadFile, (const std::string & file_path), (const, override));
 };
 
-class SystemMonitorNodeWrapper : public panther_diagnostics::SystemMonitorNode
+class SystemMonitorNodeWrapper : public husarion_ugv_diagnostics::SystemMonitorNode
 {
 public:
   SystemMonitorNodeWrapper(std::shared_ptr<MockFilesystem> filesystem)
-  : panther_diagnostics::SystemMonitorNode("test_system_statics", filesystem)
+  : husarion_ugv_diagnostics::SystemMonitorNode("test_system_statics", filesystem)
   {
   }
 
   std::vector<float> GetCoresUsages() const
   {
-    return panther_diagnostics::SystemMonitorNode::GetCoresUsages();
+    return husarion_ugv_diagnostics::SystemMonitorNode::GetCoresUsages();
   }
 
   float GetCPUMeanUsage(const std::vector<float> & usages) const
   {
-    return panther_diagnostics::SystemMonitorNode::GetCPUMeanUsage(usages);
+    return husarion_ugv_diagnostics::SystemMonitorNode::GetCPUMeanUsage(usages);
   }
 
   float GetCPUTemperature() const
   {
-    return panther_diagnostics::SystemMonitorNode::GetCPUTemperature();
+    return husarion_ugv_diagnostics::SystemMonitorNode::GetCPUTemperature();
   }
 
-  float GetRAMUsage() const { return panther_diagnostics::SystemMonitorNode::GetRAMUsage(); }
+  float GetRAMUsage() const { return husarion_ugv_diagnostics::SystemMonitorNode::GetRAMUsage(); }
 
-  float GetDiskUsage() const { return panther_diagnostics::SystemMonitorNode::GetDiskUsage(); }
+  float GetDiskUsage() const { return husarion_ugv_diagnostics::SystemMonitorNode::GetDiskUsage(); }
 
   panther_msgs::msg::SystemStatus SystemStatusToMessage(
-    const panther_diagnostics::SystemStatus & status)
+    const husarion_ugv_diagnostics::SystemStatus & status)
 
   {
-    return panther_diagnostics::SystemMonitorNode::SystemStatusToMessage(status);
+    return husarion_ugv_diagnostics::SystemMonitorNode::SystemStatusToMessage(status);
   }
 };
 
@@ -160,7 +160,7 @@ TEST_F(TestSystemMonitorNode, GetDiskUsageInvalidFilesystem)
 
 TEST_F(TestSystemMonitorNode, SystemStatusToMessage)
 {
-  panther_diagnostics::SystemStatus test_status;
+  husarion_ugv_diagnostics::SystemStatus test_status;
   test_status.core_usages = {50.0, 50.0, 50.0};
   test_status.cpu_mean_usage = 50.0;
   test_status.cpu_temperature = 36.6;
diff --git a/panther_gazebo/CHANGELOG.rst b/husarion_ugv_gazebo/CHANGELOG.rst
similarity index 100%
rename from panther_gazebo/CHANGELOG.rst
rename to husarion_ugv_gazebo/CHANGELOG.rst
diff --git a/panther_gazebo/CMakeLists.txt b/husarion_ugv_gazebo/CMakeLists.txt
similarity index 79%
rename from panther_gazebo/CMakeLists.txt
rename to husarion_ugv_gazebo/CMakeLists.txt
index cae0aaa3..962f6db5 100644
--- a/panther_gazebo/CMakeLists.txt
+++ b/husarion_ugv_gazebo/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10.2)
-project(panther_gazebo)
+project(husarion_ugv_gazebo)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
@@ -28,21 +28,21 @@ find_package(Qt5 REQUIRED COMPONENTS Core Quick QuickControls2)
 include_directories(include ${Qt5Core_INCLUDE_DIRS} ${Qt5Qml_INCLUDE_DIRS}
                     ${Qt5Quick_INCLUDE_DIRS} ${Qt5QuickControls2_INCLUDE_DIRS})
 
-add_library(panther_simulation_plugins SHARED src/gz_panther_system.cpp)
+add_library(estop_system SHARED src/estop_system.cpp)
 ament_target_dependencies(
-  panther_simulation_plugins
+  estop_system
   hardware_interface
   ign_ros2_control
   rclcpp
   rclcpp_lifecycle
   std_msgs
   std_srvs)
-target_link_libraries(panther_simulation_plugins ignition-gazebo6)
+target_link_libraries(estop_system ignition-gazebo6)
 
 set(CMAKE_AUTOMOC ON)
 qt5_add_resources(resources_rcc src/gui/EStop.qrc)
 
-add_library(EStop SHARED include/panther_gazebo/gui/e_stop.hpp
+add_library(EStop SHARED include/husarion_ugv_gazebo/gui/e_stop.hpp
                          src/gui/e_stop.cpp ${resources_rcc})
 ament_target_dependencies(EStop rclcpp std_srvs)
 target_link_libraries(
@@ -60,7 +60,7 @@ target_link_libraries(LEDStrip ignition-gazebo6 ignition-msgs8 ignition-plugin1
                       ignition-transport11)
 
 install(
-  TARGETS panther_simulation_plugins
+  TARGETS estop_system
   RUNTIME DESTINATION bin
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib)
@@ -86,10 +86,8 @@ endif()
 
 ament_export_include_directories(include)
 
-pluginlib_export_plugin_description_file(ign_ros2_control
-                                         panther_simulation_plugins.xml)
+pluginlib_export_plugin_description_file(ign_ros2_control plugins.xml)
 
-ament_environment_hooks(
-  "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in")
+ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/setup_envs.sh.in")
 
 ament_package()
diff --git a/panther_gazebo/CONFIGURATION.md b/husarion_ugv_gazebo/CONFIGURATION.md
similarity index 71%
rename from panther_gazebo/CONFIGURATION.md
rename to husarion_ugv_gazebo/CONFIGURATION.md
index d83baf52..3ad174b3 100644
--- a/panther_gazebo/CONFIGURATION.md
+++ b/husarion_ugv_gazebo/CONFIGURATION.md
@@ -1,8 +1,8 @@
-# panther_gazebo
+# husarion_ugv_gazebo
 
 ## Use of GPS in Simulation
 
-The NavSat system is utilized to publish the Panther robot's position within the Gazebo world. It manages navigation satellite sensors, such as GPS, which report position and velocity in spherical coordinates (latitude/longitude) through Ignition Transport.
+The NavSat system is utilized to publish the robot's position within the Gazebo world. It manages navigation satellite sensors, such as GPS, which report position and velocity in spherical coordinates (latitude/longitude) through Ignition Transport.
 
 The NavSat sensors requires the spherical coordinates of the world origin to be configured. This configuration can be accomplished, for instance, by employing the `<spherical_coordinates>` tag within the world's SDF or by utilizing the Ignition `/world/world_name/set_spherical_coordinates` service.
 
@@ -33,12 +33,12 @@ To obtain GPS data in Ignition, follow these steps:
 
 ## Linear Battery Plugin
 
-It is possible to simulate the battery operation of the Panther robot. By default, this feature is disabled, but you can enable it by setting the `simulate_discharging` parameter to `true` in the `battery_plugin.yaml` file or in the file pointed to by the `battery_config_path` parameter. Below, you will find the plugin parameters that enable battery simulation.
+It is possible to simulate the battery operation of the simulated robot. By default, this feature is disabled, but you can enable it by setting the `simulate_discharging` parameter to `true` in the `battery_plugin.yaml` file or in the file pointed to by the `battery_config_path` parameter. Below, you will find the plugin parameters that enable battery simulation.
 
 - `simulate_discharging` [*bool*, default: **false**]: Enables battery simulation. If set to `true`, the battery will discharge **at a constant rate** (regardless of joint torque), and if it depletes completely, the robot will stop moving. When set to `false`, the battery will not discharge, but the battery status information will still be published on the `battery/battery_status` topic.
 - `initial_charge_percentage` [*float*, default: **70.0**]: Sets the initial charge percentage of the battery.
 - `charging_time` [*float*, default: **6.0**]: Specifies the charging time for the battery in hours.
-- `power_load` [*float*, default: **120.0**]: Represents the average power load during normal operation **[W]** and is initially set to 120.0 W. With the default power load of 120.0 W, the robot can operate for up to 8 hours. When the `simulate_discharging` parameter is set to `false`, this value defaults to 0.0 W. Users are encouraged to customize this value to match their specific requirements. For more information on Panther power consumption, please refer to [Panther Battery & Charging Documentation](https://husarion.com/manuals/panther/#battery--charging).
+- `power_load` [*float*, default: **120.0**]: Represents the average power load during normal operation **[W]** and is initially set to 120.0 W. With the default power load of 120.0 W, the robot can operate for up to 8 hours. When the `simulate_discharging` parameter is set to `false`, this value defaults to 0.0 W. Users are encouraged to customize this value to match their specific requirements. For more information on robot power consumption, please refer to [Battery & Charging Documentation](https://husarion.com/manuals/panther/#battery--charging).
 
 > [!NOTE]
 >
@@ -51,11 +51,11 @@ Unfortunately, there is no straightforward way to exchange `LinearBatteryPlugin`
 You can start the charging process by calling the Ignition service:
 
 ```bash
-ign service --service /model/panther/battery/battery/recharge/start --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0
+ign service --service /model/{robot_model}/battery/battery/recharge/start --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0
 ```
 
 and stop it using:
 
 ```bash
-ign service --service /model/panther/battery/battery/recharge/stop --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0
+ign service --service /model/{robot_model}/battery/battery/recharge/stop --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0
 ```
diff --git a/panther_gazebo/README.md b/husarion_ugv_gazebo/README.md
similarity index 98%
rename from panther_gazebo/README.md
rename to husarion_ugv_gazebo/README.md
index 7a818915..3b573404 100644
--- a/panther_gazebo/README.md
+++ b/husarion_ugv_gazebo/README.md
@@ -1,4 +1,4 @@
-# panther_gazebo
+# husarion_ugv_gazebo
 
 The package contains a launch file and source files used to run the robot simulation in Gazebo. The simulator tries to reproduce the behavior of a real robot as much as possible, including the provision of an analogous ROS_API.
 
@@ -26,7 +26,7 @@ The package contains a launch file and source files used to run the robot simula
 - `hardware/e_stop_reset` [*std_srvs/Trigger*]: Resets E-stop.
 - `hardware/e_stop_trigger` [*std_srvs/Trigger*]: Triggers E-stop.
 
-### GzPantherSystem
+### EStopSystem
 
 Plugin based on `ign_system` is responsible for handling sensor interfaces (only IMU for now) and sending requests for joints compatible with `ros2_control`. Plugin also adds E-Stop support.
 
diff --git a/panther_gazebo/config/battery_plugin.yaml b/husarion_ugv_gazebo/config/battery_plugin.yaml
similarity index 82%
rename from panther_gazebo/config/battery_plugin.yaml
rename to husarion_ugv_gazebo/config/battery_plugin.yaml
index 45eaac5e..2564ebc3 100644
--- a/panther_gazebo/config/battery_plugin.yaml
+++ b/husarion_ugv_gazebo/config/battery_plugin.yaml
@@ -1,6 +1,6 @@
 # Parameters for the Gazebo LinearBatteryPlugin
-# For more information on Panther power consumption, please refer to:
-# https://husarion.com/manuals/panther/#battery--charging
+# For more information on robots power consumption, please refer to:
+# Panther: https://husarion.com/manuals/panther/#battery--charging
 
 # If set to true, the battery will discharge and if it depletes completely, the robot will stop moving.
 simulate_discharging: false
diff --git a/panther_gazebo/config/gz_bridge.yaml b/husarion_ugv_gazebo/config/gz_bridge.yaml
similarity index 100%
rename from panther_gazebo/config/gz_bridge.yaml
rename to husarion_ugv_gazebo/config/gz_bridge.yaml
diff --git a/panther_gazebo/config/teleop_with_estop.config b/husarion_ugv_gazebo/config/teleop_with_estop.config
similarity index 100%
rename from panther_gazebo/config/teleop_with_estop.config
rename to husarion_ugv_gazebo/config/teleop_with_estop.config
diff --git a/panther_gazebo/hooks/panther_gazebo.sh.in b/husarion_ugv_gazebo/hooks/setup_envs.sh.in
similarity index 100%
rename from panther_gazebo/hooks/panther_gazebo.sh.in
rename to husarion_ugv_gazebo/hooks/setup_envs.sh.in
diff --git a/panther_gazebo/include/panther_gazebo/gz_panther_system.hpp b/husarion_ugv_gazebo/include/husarion_ugv_gazebo/estop_system.hpp
similarity index 87%
rename from panther_gazebo/include/panther_gazebo/gz_panther_system.hpp
rename to husarion_ugv_gazebo/include/husarion_ugv_gazebo/estop_system.hpp
index 6978fa71..6d8883c7 100644
--- a/panther_gazebo/include/panther_gazebo/gz_panther_system.hpp
+++ b/husarion_ugv_gazebo/include/husarion_ugv_gazebo/estop_system.hpp
@@ -13,8 +13,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_GAZEBO_GZ_PANTHER_SYSTEM
-#define PANTHER_GAZEBO_GZ_PANTHER_SYSTEM
+#ifndef HUSARION_UGV_GAZEBO_ESTOP_SYSTEM
+#define HUSARION_UGV_GAZEBO_ESTOP_SYSTEM
 
 #include <memory>
 
@@ -27,18 +27,18 @@
 #include <std_msgs/msg/bool.hpp>
 #include <std_srvs/srv/trigger.hpp>
 
-namespace panther_gazebo
+namespace husarion_ugv_gazebo
 {
 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 using BoolMsg = std_msgs::msg::Bool;
 using TriggerSrv = std_srvs::srv::Trigger;
 
 /**
- * @brief Main class for the Panther System which implements a simulated `ros2_control`
+ * @brief Main class for the Husarion UGV which implements a simulated `ros2_control`
  * `hardware_interface::SystemInterface`. This class inherits `ign_ros2_control::IgnitionSystem`
  * and implements additional functionalities like E-stop handling.
  */
-class GzPantherSystem : public ign_ros2_control::IgnitionSystem
+class EStopSystem : public ign_ros2_control::IgnitionSystem
 {
 public:
   CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info) override;
@@ -69,6 +69,6 @@ class GzPantherSystem : public ign_ros2_control::IgnitionSystem
   rclcpp::Service<TriggerSrv>::SharedPtr e_stop_trigger_service_;
 };
 
-}  // namespace panther_gazebo
+}  // namespace husarion_ugv_gazebo
 
-#endif  // PANTHER_GAZEBO_GZ_PANTHER_SYSTEM
+#endif  // HUSARION_UGV_GAZEBO_ESTOP_SYSTEM
diff --git a/panther_gazebo/include/panther_gazebo/gui/e_stop.hpp b/husarion_ugv_gazebo/include/husarion_ugv_gazebo/gui/e_stop.hpp
similarity index 89%
rename from panther_gazebo/include/panther_gazebo/gui/e_stop.hpp
rename to husarion_ugv_gazebo/include/husarion_ugv_gazebo/gui/e_stop.hpp
index 055db7e1..962f5b20 100644
--- a/panther_gazebo/include/panther_gazebo/gui/e_stop.hpp
+++ b/husarion_ugv_gazebo/include/husarion_ugv_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_GUI_E_STOP_HPP_
-#define PANTHER_GAZEBO_GUI_E_STOP_HPP_
+#ifndef HUSARION_UGV_GAZEBO_GUI_E_STOP_HPP_
+#define HUSARION_UGV_GAZEBO_GUI_E_STOP_HPP_
 
 #include <string>
 
@@ -23,7 +23,7 @@
 
 #include <std_srvs/srv/trigger.hpp>
 
-namespace panther_gazebo
+namespace husarion_ugv_gazebo
 {
 
 class EStop : public ignition::gui::Plugin
@@ -59,6 +59,6 @@ protected slots:
   rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr e_stop_reset_client_;
   rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr e_stop_trigger_client_;
 };
-}  // namespace panther_gazebo
+}  // namespace husarion_ugv_gazebo
 
-#endif  // PANTHER_GAZEBO_GUI_E_STOP_HPP_
+#endif  // HUSARION_UGV_GAZEBO_GUI_E_STOP_HPP_
diff --git a/panther_gazebo/include/panther_gazebo/led_strip.hpp b/husarion_ugv_gazebo/include/husarion_ugv_gazebo/led_strip.hpp
similarity index 96%
rename from panther_gazebo/include/panther_gazebo/led_strip.hpp
rename to husarion_ugv_gazebo/include/husarion_ugv_gazebo/led_strip.hpp
index 0f0945ea..74d76444 100644
--- a/panther_gazebo/include/panther_gazebo/led_strip.hpp
+++ b/husarion_ugv_gazebo/include/husarion_ugv_gazebo/led_strip.hpp
@@ -12,8 +12,8 @@
 // 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_
+#ifndef HUSARION_UGV_GAZEBO_LED_STRIP_HPP_
+#define HUSARION_UGV_GAZEBO_LED_STRIP_HPP_
 
 #include <chrono>
 #include <string>
@@ -33,7 +33,7 @@
 #include <gz/msgs/image.pb.h>
 #include <gz/msgs/light.pb.h>
 
-namespace panther_gazebo
+namespace husarion_ugv_gazebo
 {
 
 /**
@@ -144,6 +144,6 @@ class LEDStrip : public gz::sim::System,
     1)};  // Avoid initialization errors when the robot is not yet spawned on the scene.
 };
 
-}  // namespace panther_gazebo
+}  // namespace husarion_ugv_gazebo
 
-#endif  // PANTHER_GAZEBO_LED_STRIP_HPP_
+#endif  // HUSARION_UGV_GAZEBO_LED_STRIP_HPP_
diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/husarion_ugv_gazebo/launch/simulate_robot.launch.py
similarity index 91%
rename from panther_gazebo/launch/simulate_robot.launch.py
rename to husarion_ugv_gazebo/launch/simulate_robot.launch.py
index fdde5156..db367687 100644
--- a/panther_gazebo/launch/simulate_robot.launch.py
+++ b/husarion_ugv_gazebo/launch/simulate_robot.launch.py
@@ -50,7 +50,7 @@ def generate_launch_description():
             "This configuration is intended for use in simulations only."
         ),
         default_value=PathJoinSubstitution(
-            [FindPackageShare("panther_gazebo"), "config", "battery_plugin.yaml"]
+            [FindPackageShare("husarion_ugv_gazebo"), "config", "battery_plugin.yaml"]
         ),
     )
 
@@ -64,8 +64,8 @@ def generate_launch_description():
         ),
         description=(
             "Additional components configuration file. Components described in this file "
-            "are dynamically included in Panther's urdf."
-            "Panther options are described here "
+            "are dynamically included in robot's URDF."
+            "Available options are described in the manual: "
             "https://husarion.com/manuals/panther/panther-options/"
         ),
     )
@@ -82,7 +82,7 @@ def generate_launch_description():
     declare_gz_bridge_config_path_arg = DeclareLaunchArgument(
         "gz_bridge_config_path",
         default_value=PathJoinSubstitution(
-            [FindPackageShare("panther_gazebo"), "config", "gz_bridge.yaml"]
+            [FindPackageShare("husarion_ugv_gazebo"), "config", "gz_bridge.yaml"]
         ),
         description="Path to the parameter_bridge configuration file.",
     )
@@ -106,7 +106,7 @@ def generate_launch_description():
     spawn_robot_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare("panther_gazebo"), "launch", "spawn_robot.launch.py"]
+                [FindPackageShare("husarion_ugv_gazebo"), "launch", "spawn_robot.launch.py"]
             )
         ),
         launch_arguments={
@@ -118,7 +118,7 @@ def generate_launch_description():
     lights_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare("panther_lights"), "launch", "lights.launch.py"]
+                [FindPackageShare("husarion_ugv_lights"), "launch", "lights.launch.py"]
             )
         ),
         launch_arguments={"namespace": namespace, "use_sim": "True"}.items(),
@@ -127,7 +127,7 @@ def generate_launch_description():
     manager_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare("panther_manager"), "launch", "manager.launch.py"]
+                [FindPackageShare("husarion_ugv_manager"), "launch", "manager.launch.py"]
             )
         ),
         launch_arguments={"namespace": namespace, "use_sim": "True"}.items(),
@@ -138,7 +138,7 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
                 [
-                    FindPackageShare("panther_controller"),
+                    FindPackageShare("husarion_ugv_controller"),
                     "launch",
                     "controller.launch.py",
                 ]
@@ -155,7 +155,7 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
                 [
-                    FindPackageShare("panther_localization"),
+                    FindPackageShare("husarion_ugv_localization"),
                     "launch",
                     "localization.launch.py",
                 ]
@@ -191,7 +191,7 @@ def generate_launch_description():
     gz_bridge = Node(
         package="ros_gz_bridge",
         executable="parameter_bridge",
-        name="panther_base_gz_bridge",
+        name="gz_bridge",
         parameters=[{"config_file": namespaced_gz_bridge_config_path}],
         namespace=namespace,
         emulate_tty=True,
diff --git a/panther_gazebo/launch/simulation.launch.py b/husarion_ugv_gazebo/launch/simulation.launch.py
similarity index 94%
rename from panther_gazebo/launch/simulation.launch.py
rename to husarion_ugv_gazebo/launch/simulation.launch.py
index 5ea18720..cf2e78de 100644
--- a/panther_gazebo/launch/simulation.launch.py
+++ b/husarion_ugv_gazebo/launch/simulation.launch.py
@@ -33,7 +33,7 @@ def generate_launch_description():
     declare_gz_gui = DeclareLaunchArgument(
         "gz_gui",
         default_value=PathJoinSubstitution(
-            [FindPackageShare("panther_gazebo"), "config", "teleop_with_estop.config"]
+            [FindPackageShare("husarion_ugv_gazebo"), "config", "teleop_with_estop.config"]
         ),
         description="Run simulation with specific GUI layout.",
     )
@@ -63,7 +63,7 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
                 [
-                    FindPackageShare("panther_gazebo"),
+                    FindPackageShare("husarion_ugv_gazebo"),
                     "launch",
                     "simulate_robot.launch.py",
                 ]
diff --git a/panther_gazebo/launch/spawn_robot.launch.py b/husarion_ugv_gazebo/launch/spawn_robot.launch.py
similarity index 98%
rename from panther_gazebo/launch/spawn_robot.launch.py
rename to husarion_ugv_gazebo/launch/spawn_robot.launch.py
index 624586ba..39a47a2c 100644
--- a/panther_gazebo/launch/spawn_robot.launch.py
+++ b/husarion_ugv_gazebo/launch/spawn_robot.launch.py
@@ -16,6 +16,7 @@
 
 import os
 
+from husarion_ugv_utils.messages import welcome_msg
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -27,7 +28,6 @@
 )
 from launch_ros.actions import Node, SetUseSimTime
 from launch_ros.substitutions import FindPackageShare
-from panther_utils.messages import welcome_msg
 
 
 def generate_launch_description():
diff --git a/panther_gazebo/package.xml b/husarion_ugv_gazebo/package.xml
similarity index 91%
rename from panther_gazebo/package.xml
rename to husarion_ugv_gazebo/package.xml
index faa23cdd..7f76d0f7 100644
--- a/panther_gazebo/package.xml
+++ b/husarion_ugv_gazebo/package.xml
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>panther_gazebo</name>
+  <name>husarion_ugv_gazebo</name>
   <version>2.1.1</version>
-  <description>The panther_description package</description>
+  <description>The packages simulating behaviour of the Husarion robots</description>
   <maintainer email="support@husarion.com">Husarion</maintainer>
   <license>Apache License 2.0</license>
 
@@ -29,16 +29,16 @@
 
   <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>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">husarion_ugv_controller</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">husarion_ugv_lights</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">husarion_ugv_localization</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">husarion_ugv_manager</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">husarion_ugv_utils</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">launch</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">launch_ros</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">lynx_description</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">nav2_common</exec_depend>
-  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_controller</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_description</exec_depend>
-  <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>
diff --git a/panther_gazebo/panther_simulation_plugins.xml b/husarion_ugv_gazebo/plugins.xml
similarity index 62%
rename from panther_gazebo/panther_simulation_plugins.xml
rename to husarion_ugv_gazebo/plugins.xml
index 7a0aef76..f97c60b1 100644
--- a/panther_gazebo/panther_simulation_plugins.xml
+++ b/husarion_ugv_gazebo/plugins.xml
@@ -1,7 +1,7 @@
-<library path="panther_simulation_plugins">
+<library path="estop_system">
   <class
-    name="panther_gazebo/GzPantherSystem"
-    type="panther_gazebo::GzPantherSystem"
+    name="husarion_ugv_gazebo/EStopSystem"
+    type="husarion_ugv_gazebo::EStopSystem"
     base_class_type="ign_ros2_control::IgnitionSystemInterface">
     <description>
       Control joints position with E-Stop functionalities in Gazebo using ROS2.
diff --git a/panther_gazebo/src/gz_panther_system.cpp b/husarion_ugv_gazebo/src/estop_system.cpp
similarity index 82%
rename from panther_gazebo/src/gz_panther_system.cpp
rename to husarion_ugv_gazebo/src/estop_system.cpp
index d705d716..4f37923f 100644
--- a/panther_gazebo/src/gz_panther_system.cpp
+++ b/husarion_ugv_gazebo/src/estop_system.cpp
@@ -28,7 +28,7 @@
 #include <hardware_interface/hardware_info.hpp>
 #include <hardware_interface/lexical_casts.hpp>
 
-#include "panther_gazebo/gz_panther_system.hpp"
+#include "husarion_ugv_gazebo/estop_system.hpp"
 
 struct jointData
 {
@@ -94,10 +94,10 @@ class ign_ros2_control::IgnitionSystemPrivate
   double position_proportional_gain_;
 };
 
-namespace panther_gazebo
+namespace husarion_ugv_gazebo
 {
 
-CallbackReturn GzPantherSystem::on_init(const hardware_interface::HardwareInfo & system_info)
+CallbackReturn EStopSystem::on_init(const hardware_interface::HardwareInfo & system_info)
 {
   if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) {
     return CallbackReturn::ERROR;
@@ -108,24 +108,24 @@ CallbackReturn GzPantherSystem::on_init(const hardware_interface::HardwareInfo &
   return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn GzPantherSystem::on_configure(const rclcpp_lifecycle::State & previous_state)
+CallbackReturn EStopSystem::on_configure(const rclcpp_lifecycle::State & previous_state)
 {
   SetupEStop();
   return hardware_interface::SystemInterface::on_configure(previous_state);
 }
 
-CallbackReturn GzPantherSystem::on_activate(const rclcpp_lifecycle::State & previous_state)
+CallbackReturn EStopSystem::on_activate(const rclcpp_lifecycle::State & previous_state)
 {
   PublishEStopStatus();
   return hardware_interface::SystemInterface::on_activate(previous_state);
 }
 
-CallbackReturn GzPantherSystem::on_deactivate(const rclcpp_lifecycle::State & previous_state)
+CallbackReturn EStopSystem::on_deactivate(const rclcpp_lifecycle::State & previous_state)
 {
   return hardware_interface::SystemInterface::on_deactivate(previous_state);
 }
 
-hardware_interface::return_type GzPantherSystem::write(
+hardware_interface::return_type EStopSystem::write(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
   if (e_stop_active_) {
@@ -135,7 +135,7 @@ hardware_interface::return_type GzPantherSystem::write(
   return IgnitionSystem::write(time, period);
 }
 
-void GzPantherSystem::SetupEStop()
+void EStopSystem::SetupEStop()
 {
   e_stop_publisher_ = nh_->create_publisher<BoolMsg>(
     "~/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
@@ -143,22 +143,22 @@ void GzPantherSystem::SetupEStop()
   e_stop_reset_service_ = nh_->create_service<TriggerSrv>(
     "~/e_stop_reset",
     std::bind(
-      &GzPantherSystem::EStopResetCallback, this, std::placeholders::_1, std::placeholders::_2));
+      &EStopSystem::EStopResetCallback, this, std::placeholders::_1, std::placeholders::_2));
 
   e_stop_trigger_service_ = nh_->create_service<TriggerSrv>(
     "~/e_stop_trigger",
     std::bind(
-      &GzPantherSystem::EStopTriggerCallback, this, std::placeholders::_1, std::placeholders::_2));
+      &EStopSystem::EStopTriggerCallback, this, std::placeholders::_1, std::placeholders::_2));
 }
 
-void GzPantherSystem::PublishEStopStatus()
+void EStopSystem::PublishEStopStatus()
 {
   std_msgs::msg::Bool e_stop_msg;
   e_stop_msg.data = e_stop_active_;
   e_stop_publisher_->publish(e_stop_msg);
 }
 
-void GzPantherSystem::EStopResetCallback(
+void EStopSystem::EStopResetCallback(
   const TriggerSrv::Request::SharedPtr & /*request*/, TriggerSrv::Response::SharedPtr response)
 {
   e_stop_active_ = false;
@@ -167,7 +167,7 @@ void GzPantherSystem::EStopResetCallback(
   PublishEStopStatus();
 }
 
-void GzPantherSystem::EStopTriggerCallback(
+void EStopSystem::EStopTriggerCallback(
   const TriggerSrv::Request::SharedPtr & /*request*/, TriggerSrv::Response::SharedPtr response)
 {
   e_stop_active_ = true;
@@ -176,7 +176,7 @@ void GzPantherSystem::EStopTriggerCallback(
   PublishEStopStatus();
 }
 
-}  // namespace panther_gazebo
+}  // namespace husarion_ugv_gazebo
 
 #include "pluginlib/class_list_macros.hpp"  // NOLINT
-PLUGINLIB_EXPORT_CLASS(panther_gazebo::GzPantherSystem, ign_ros2_control::IgnitionSystemInterface)
+PLUGINLIB_EXPORT_CLASS(husarion_ugv_gazebo::EStopSystem, ign_ros2_control::IgnitionSystemInterface)
diff --git a/panther_gazebo/src/gui/EStop.qml b/husarion_ugv_gazebo/src/gui/EStop.qml
similarity index 100%
rename from panther_gazebo/src/gui/EStop.qml
rename to husarion_ugv_gazebo/src/gui/EStop.qml
diff --git a/panther_gazebo/src/gui/EStop.qrc b/husarion_ugv_gazebo/src/gui/EStop.qrc
similarity index 100%
rename from panther_gazebo/src/gui/EStop.qrc
rename to husarion_ugv_gazebo/src/gui/EStop.qrc
diff --git a/panther_gazebo/src/gui/e_stop.cpp b/husarion_ugv_gazebo/src/gui/e_stop.cpp
similarity index 94%
rename from panther_gazebo/src/gui/e_stop.cpp
rename to husarion_ugv_gazebo/src/gui/e_stop.cpp
index ff4c3010..ffad684d 100644
--- a/panther_gazebo/src/gui/e_stop.cpp
+++ b/husarion_ugv_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/gui/e_stop.hpp"
+#include "husarion_ugv_gazebo/gui/e_stop.hpp"
 
 #include <memory>
 
@@ -23,7 +23,7 @@
 
 #include <std_srvs/srv/trigger.hpp>
 
-namespace panther_gazebo
+namespace husarion_ugv_gazebo
 {
 
 EStop::EStop() : ignition::gui::Plugin() { rclcpp::init(0, nullptr); }
@@ -90,6 +90,6 @@ void EStop::SetNamespace(const QString & ns)
   ignmsg << "Changed namespace to: " << namespace_ << std::endl;
 }
 
-}  // namespace panther_gazebo
+}  // namespace husarion_ugv_gazebo
 
-IGNITION_ADD_PLUGIN(panther_gazebo::EStop, ignition::gui::Plugin)
+IGNITION_ADD_PLUGIN(husarion_ugv_gazebo::EStop, ignition::gui::Plugin)
diff --git a/panther_gazebo/src/led_strip.cpp b/husarion_ugv_gazebo/src/led_strip.cpp
similarity index 97%
rename from panther_gazebo/src/led_strip.cpp
rename to husarion_ugv_gazebo/src/led_strip.cpp
index c7820bb1..c0984620 100644
--- a/panther_gazebo/src/led_strip.cpp
+++ b/husarion_ugv_gazebo/src/led_strip.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_gazebo/led_strip.hpp"
+#include "husarion_ugv_gazebo/led_strip.hpp"
 
 #include <cstddef>
 #include <limits>
@@ -25,7 +25,7 @@
 #include <gz/msgs/color.pb.h>
 #include <gz/msgs/marker.pb.h>
 
-namespace panther_gazebo
+namespace husarion_ugv_gazebo
 {
 void LEDStrip::Configure(
   const gz::sim::Entity & entity, const std::shared_ptr<const sdf::Element> & sdf,
@@ -275,8 +275,8 @@ void LEDStrip::CreateMarker(
   node_.Request("/marker", marker_msg);
 }
 
-}  // namespace panther_gazebo
+}  // namespace husarion_ugv_gazebo
 
 IGNITION_ADD_PLUGIN(
-  panther_gazebo::LEDStrip, gz::sim::System, panther_gazebo::LEDStrip::ISystemConfigure,
-  panther_gazebo::LEDStrip::ISystemPreUpdate)
+  husarion_ugv_gazebo::LEDStrip, gz::sim::System, husarion_ugv_gazebo::LEDStrip::ISystemConfigure,
+  husarion_ugv_gazebo::LEDStrip::ISystemPreUpdate)
diff --git a/panther_hardware_interfaces/CHANGELOG.rst b/husarion_ugv_hardware_interfaces/CHANGELOG.rst
similarity index 100%
rename from panther_hardware_interfaces/CHANGELOG.rst
rename to husarion_ugv_hardware_interfaces/CHANGELOG.rst
diff --git a/panther_hardware_interfaces/CMakeLists.txt b/husarion_ugv_hardware_interfaces/CMakeLists.txt
similarity index 58%
rename from panther_hardware_interfaces/CMakeLists.txt
rename to husarion_ugv_hardware_interfaces/CMakeLists.txt
index 1728bd37..66d2aeff 100644
--- a/panther_hardware_interfaces/CMakeLists.txt
+++ b/husarion_ugv_hardware_interfaces/CMakeLists.txt
@@ -8,7 +8,7 @@ if(USE_SUPERBUILD)
   include(cmake/SuperBuild.cmake)
   return()
 else()
-  project(panther_hardware_interfaces)
+  project(husarion_ugv_hardware_interfaces)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -24,7 +24,7 @@ set(PACKAGE_DEPENDENCIES
     hardware_interface
     imu_filter_madgwick
     panther_msgs
-    panther_utils
+    husarion_ugv_utils
     phidgets_api
     PkgConfig
     pluginlib
@@ -52,31 +52,31 @@ pkg_check_modules(LIBGPIOD REQUIRED IMPORTED_TARGET libgpiodcxx)
 
 add_library(
   ${PROJECT_NAME} SHARED
-  src/panther_imu_sensor/panther_imu_sensor.cpp
-  src/panther_system/gpio/gpio_controller.cpp
-  src/panther_system/gpio/gpio_driver.cpp
-  src/panther_system/robot_driver/canopen_manager.cpp
-  src/panther_system/robot_driver/roboteq_data_converters.cpp
-  src/panther_system/robot_driver/roboteq_driver.cpp
-  src/panther_system/robot_driver/roboteq_error_filter.cpp
-  src/panther_system/robot_driver/roboteq_robot_driver.cpp
-  src/panther_system/robot_driver/lynx_robot_driver.cpp
-  src/panther_system/robot_driver/panther_robot_driver.cpp
-  src/panther_system/system_e_stop.cpp
-  src/panther_system/system_ros_interface.cpp
-  src/panther_system/lynx_system.cpp
-  src/panther_system/panther_system.cpp
-  src/panther_system/ugv_system.cpp
+  src/phidget_imu_sensor/phidget_imu_sensor.cpp
+  src/robot_system/gpio/gpio_controller.cpp
+  src/robot_system/gpio/gpio_driver.cpp
+  src/robot_system/robot_driver/canopen_manager.cpp
+  src/robot_system/robot_driver/roboteq_data_converters.cpp
+  src/robot_system/robot_driver/roboteq_driver.cpp
+  src/robot_system/robot_driver/roboteq_error_filter.cpp
+  src/robot_system/robot_driver/roboteq_robot_driver.cpp
+  src/robot_system/robot_driver/lynx_robot_driver.cpp
+  src/robot_system/robot_driver/panther_robot_driver.cpp
+  src/robot_system/system_e_stop.cpp
+  src/robot_system/system_ros_interface.cpp
+  src/robot_system/lynx_system.cpp
+  src/robot_system/panther_system.cpp
+  src/robot_system/ugv_system.cpp
   src/utils.cpp)
 ament_target_dependencies(${PROJECT_NAME} ${PACKAGE_DEPENDENCIES})
 target_link_libraries(${PROJECT_NAME} PkgConfig::LIBLELY_COAPP
                       PkgConfig::LIBGPIOD phidgets_spatial_parameters)
 
-target_compile_definitions(${PROJECT_NAME}
-                           PRIVATE "PANTHER_HARDWARE_INTERFACES_BUILDING_DLL")
+target_compile_definitions(
+  ${PROJECT_NAME} PRIVATE "HUSARION_UGV_HARDWARE_INTERFACES_BUILDING_DLL")
 
 pluginlib_export_plugin_description_file(hardware_interface
-                                         panther_hardware_interfaces.xml)
+                                         husarion_ugv_hardware_interfaces.xml)
 
 install(
   TARGETS ${PROJECT_NAME}
@@ -90,134 +90,135 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(ros_testing REQUIRED)
-  find_package(panther_utils REQUIRED)
+  find_package(husarion_ugv_utils REQUIRED)
 
   install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)
 
   ament_add_gtest(${PROJECT_NAME}_test_utils test/test_utils.cpp src/utils.cpp)
 
-  ament_add_gtest(${PROJECT_NAME}_test_panther_imu_sensor
-                  test/panther_imu_sensor/test_panther_imu_sensor.cpp)
+  ament_add_gtest(${PROJECT_NAME}_test_phidget_imu_sensor
+                  test/phidget_imu_sensor/test_phidget_imu_sensor.cpp)
   ament_target_dependencies(
-    ${PROJECT_NAME}_test_panther_imu_sensor hardware_interface rclcpp
-    panther_utils panther_msgs phidgets_api)
-  target_link_libraries(${PROJECT_NAME}_test_panther_imu_sensor ${PROJECT_NAME}
+    ${PROJECT_NAME}_test_phidget_imu_sensor hardware_interface rclcpp
+    husarion_ugv_utils panther_msgs phidgets_api)
+  target_link_libraries(${PROJECT_NAME}_test_phidget_imu_sensor ${PROJECT_NAME}
                         phidgets_spatial_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_error_filter
-    test/unit/panther_system/robot_driver/test_roboteq_error_filter.cpp
-    src/panther_system/robot_driver/roboteq_error_filter.cpp)
+    test/unit/robot_system/robot_driver/test_roboteq_error_filter.cpp
+    src/robot_system/robot_driver/roboteq_error_filter.cpp)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_data_converters
-    test/unit/panther_system/robot_driver/test_roboteq_data_converters.cpp
-    src/panther_system/robot_driver/roboteq_data_converters.cpp src/utils.cpp)
+    test/unit/robot_system/robot_driver/test_roboteq_data_converters.cpp
+    src/robot_system/robot_driver/roboteq_data_converters.cpp src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_roboteq_data_converters
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_roboteq_data_converters
-                            panther_msgs panther_utils)
+                            panther_msgs husarion_ugv_utils)
   target_link_libraries(${PROJECT_NAME}_test_roboteq_data_converters
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_canopen_manager
-    test/unit/panther_system/robot_driver/test_canopen_manager.cpp
-    src/panther_system/robot_driver/canopen_manager.cpp src/utils.cpp)
+    test/unit/robot_system/robot_driver/test_canopen_manager.cpp
+    src/robot_system/robot_driver/canopen_manager.cpp src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_canopen_manager
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_canopen_manager rclcpp
-                            panther_msgs panther_utils)
+                            panther_msgs husarion_ugv_utils)
   target_link_libraries(${PROJECT_NAME}_test_canopen_manager
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_driver
-    test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
-    src/panther_system/robot_driver/canopen_manager.cpp
-    src/panther_system/robot_driver/roboteq_driver.cpp
+    test/unit/robot_system/robot_driver/test_roboteq_driver.cpp
+    src/robot_system/robot_driver/canopen_manager.cpp
+    src/robot_system/robot_driver/roboteq_driver.cpp
     src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_roboteq_driver
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_roboteq_driver rclcpp
-                            panther_msgs panther_utils)
+                            panther_msgs husarion_ugv_utils)
   target_link_libraries(${PROJECT_NAME}_test_roboteq_driver
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gmock(
     ${PROJECT_NAME}_test_roboteq_robot_driver
-    test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
-    src/panther_system/robot_driver/canopen_manager.cpp
-    src/panther_system/robot_driver/roboteq_driver.cpp
-    src/panther_system/robot_driver/roboteq_data_converters.cpp
-    src/panther_system/robot_driver/roboteq_robot_driver.cpp
+    test/unit/robot_system/robot_driver/test_roboteq_robot_driver.cpp
+    src/robot_system/robot_driver/canopen_manager.cpp
+    src/robot_system/robot_driver/roboteq_driver.cpp
+    src/robot_system/robot_driver/roboteq_data_converters.cpp
+    src/robot_system/robot_driver/roboteq_robot_driver.cpp
     src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_roboteq_robot_driver
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_roboteq_robot_driver rclcpp
-                            panther_msgs panther_utils)
+                            panther_msgs husarion_ugv_utils)
   target_link_libraries(${PROJECT_NAME}_test_roboteq_robot_driver
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gmock(
     ${PROJECT_NAME}_test_lynx_robot_driver
-    test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
-    src/panther_system/robot_driver/canopen_manager.cpp
-    src/panther_system/robot_driver/roboteq_driver.cpp
-    src/panther_system/robot_driver/roboteq_data_converters.cpp
-    src/panther_system/robot_driver/roboteq_robot_driver.cpp
-    src/panther_system/robot_driver/lynx_robot_driver.cpp
+    test/unit/robot_system/robot_driver/test_lynx_robot_driver.cpp
+    src/robot_system/robot_driver/canopen_manager.cpp
+    src/robot_system/robot_driver/roboteq_driver.cpp
+    src/robot_system/robot_driver/roboteq_data_converters.cpp
+    src/robot_system/robot_driver/roboteq_robot_driver.cpp
+    src/robot_system/robot_driver/lynx_robot_driver.cpp
     src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_lynx_robot_driver
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_lynx_robot_driver rclcpp
-                            panther_msgs panther_utils)
+                            panther_msgs husarion_ugv_utils)
   target_link_libraries(${PROJECT_NAME}_test_lynx_robot_driver
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gmock(
     ${PROJECT_NAME}_test_panther_robot_driver
-    test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
-    src/panther_system/robot_driver/canopen_manager.cpp
-    src/panther_system/robot_driver/roboteq_driver.cpp
-    src/panther_system/robot_driver/roboteq_data_converters.cpp
-    src/panther_system/robot_driver/roboteq_robot_driver.cpp
-    src/panther_system/robot_driver/panther_robot_driver.cpp
+    test/unit/robot_system/robot_driver/test_panther_robot_driver.cpp
+    src/robot_system/robot_driver/canopen_manager.cpp
+    src/robot_system/robot_driver/roboteq_driver.cpp
+    src/robot_system/robot_driver/roboteq_data_converters.cpp
+    src/robot_system/robot_driver/roboteq_robot_driver.cpp
+    src/robot_system/robot_driver/panther_robot_driver.cpp
     src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_panther_robot_driver
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_panther_robot_driver rclcpp
-                            panther_msgs panther_utils)
+                            panther_msgs husarion_ugv_utils)
   target_link_libraries(${PROJECT_NAME}_test_panther_robot_driver
                         PkgConfig::LIBLELY_COAPP)
 
   ament_add_gmock(
     ${PROJECT_NAME}_test_gpiod_controller
-    test/panther_system/gpio/test_gpio_controller.cpp
-    src/panther_system/gpio/gpio_controller.cpp
-    src/panther_system/gpio/gpio_driver.cpp)
-  ament_target_dependencies(${PROJECT_NAME}_test_gpiod_controller panther_utils)
+    test/robot_system/gpio/test_gpio_controller.cpp
+    src/robot_system/gpio/gpio_controller.cpp
+    src/robot_system/gpio/gpio_driver.cpp)
+  ament_target_dependencies(${PROJECT_NAME}_test_gpiod_controller
+                            husarion_ugv_utils)
   target_link_libraries(${PROJECT_NAME}_test_gpiod_controller
                         PkgConfig::LIBGPIOD)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_system_ros_interface
-    test/unit/panther_system/test_system_ros_interface.cpp
-    src/panther_system/system_ros_interface.cpp
-    src/panther_system/robot_driver/roboteq_data_converters.cpp
-    src/panther_system/gpio/gpio_controller.cpp
+    test/unit/robot_system/test_system_ros_interface.cpp
+    src/robot_system/system_ros_interface.cpp
+    src/robot_system/robot_driver/roboteq_data_converters.cpp
+    src/robot_system/gpio/gpio_controller.cpp
     src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_system_ros_interface
@@ -228,7 +229,7 @@ if(BUILD_TESTING)
     diagnostic_updater
     rclcpp
     panther_msgs
-    panther_utils
+    husarion_ugv_utils
     realtime_tools
     std_srvs)
   target_link_libraries(${PROJECT_NAME}_test_system_ros_interface
@@ -236,14 +237,14 @@ if(BUILD_TESTING)
 
   ament_add_gmock(
     ${PROJECT_NAME}_test_ugv_system
-    test/unit/panther_system/test_ugv_system.cpp
-    src/panther_system/gpio/gpio_controller.cpp
-    src/panther_system/gpio/gpio_driver.cpp
-    src/panther_system/system_e_stop.cpp
-    src/panther_system/robot_driver/roboteq_data_converters.cpp
-    src/panther_system/robot_driver/roboteq_error_filter.cpp
-    src/panther_system/system_ros_interface.cpp
-    src/panther_system/ugv_system.cpp
+    test/unit/robot_system/test_ugv_system.cpp
+    src/robot_system/gpio/gpio_controller.cpp
+    src/robot_system/gpio/gpio_driver.cpp
+    src/robot_system/system_e_stop.cpp
+    src/robot_system/robot_driver/roboteq_data_converters.cpp
+    src/robot_system/robot_driver/roboteq_error_filter.cpp
+    src/robot_system/system_ros_interface.cpp
+    src/robot_system/ugv_system.cpp
     src/utils.cpp)
   set_tests_properties(${PROJECT_NAME}_test_ugv_system PROPERTIES TIMEOUT 120)
   target_include_directories(
@@ -256,7 +257,7 @@ if(BUILD_TESTING)
     hardware_interface
     rclcpp
     panther_msgs
-    panther_utils
+    husarion_ugv_utils
     std_msgs
     std_srvs)
   target_link_libraries(${PROJECT_NAME}_test_ugv_system
@@ -264,19 +265,19 @@ if(BUILD_TESTING)
 
   ament_add_gmock(
     ${PROJECT_NAME}_test_lynx_system
-    test/unit/panther_system/test_lynx_system.cpp
-    src/panther_system/gpio/gpio_controller.cpp
-    src/panther_system/gpio/gpio_driver.cpp
-    src/panther_system/system_e_stop.cpp
-    src/panther_system/robot_driver/canopen_manager.cpp
-    src/panther_system/robot_driver/roboteq_robot_driver.cpp
-    src/panther_system/robot_driver/roboteq_data_converters.cpp
-    src/panther_system/robot_driver/roboteq_driver.cpp
-    src/panther_system/robot_driver/roboteq_error_filter.cpp
-    src/panther_system/robot_driver/lynx_robot_driver.cpp
-    src/panther_system/system_ros_interface.cpp
-    src/panther_system/lynx_system.cpp
-    src/panther_system/ugv_system.cpp
+    test/unit/robot_system/test_lynx_system.cpp
+    src/robot_system/gpio/gpio_controller.cpp
+    src/robot_system/gpio/gpio_driver.cpp
+    src/robot_system/system_e_stop.cpp
+    src/robot_system/robot_driver/canopen_manager.cpp
+    src/robot_system/robot_driver/roboteq_robot_driver.cpp
+    src/robot_system/robot_driver/roboteq_data_converters.cpp
+    src/robot_system/robot_driver/roboteq_driver.cpp
+    src/robot_system/robot_driver/roboteq_error_filter.cpp
+    src/robot_system/robot_driver/lynx_robot_driver.cpp
+    src/robot_system/system_ros_interface.cpp
+    src/robot_system/lynx_system.cpp
+    src/robot_system/ugv_system.cpp
     src/utils.cpp)
   set_tests_properties(${PROJECT_NAME}_test_lynx_system PROPERTIES TIMEOUT 120)
   target_include_directories(
@@ -289,7 +290,7 @@ if(BUILD_TESTING)
     hardware_interface
     rclcpp
     panther_msgs
-    panther_utils
+    husarion_ugv_utils
     std_msgs
     std_srvs)
   target_link_libraries(${PROJECT_NAME}_test_lynx_system
@@ -297,19 +298,19 @@ if(BUILD_TESTING)
 
   ament_add_gmock(
     ${PROJECT_NAME}_test_panther_system
-    test/unit/panther_system/test_panther_system.cpp
-    src/panther_system/gpio/gpio_controller.cpp
-    src/panther_system/gpio/gpio_driver.cpp
-    src/panther_system/system_e_stop.cpp
-    src/panther_system/robot_driver/canopen_manager.cpp
-    src/panther_system/robot_driver/roboteq_robot_driver.cpp
-    src/panther_system/robot_driver/roboteq_data_converters.cpp
-    src/panther_system/robot_driver/roboteq_driver.cpp
-    src/panther_system/robot_driver/roboteq_error_filter.cpp
-    src/panther_system/robot_driver/panther_robot_driver.cpp
-    src/panther_system/system_ros_interface.cpp
-    src/panther_system/panther_system.cpp
-    src/panther_system/ugv_system.cpp
+    test/unit/robot_system/test_panther_system.cpp
+    src/robot_system/gpio/gpio_controller.cpp
+    src/robot_system/gpio/gpio_driver.cpp
+    src/robot_system/system_e_stop.cpp
+    src/robot_system/robot_driver/canopen_manager.cpp
+    src/robot_system/robot_driver/roboteq_robot_driver.cpp
+    src/robot_system/robot_driver/roboteq_data_converters.cpp
+    src/robot_system/robot_driver/roboteq_driver.cpp
+    src/robot_system/robot_driver/roboteq_error_filter.cpp
+    src/robot_system/robot_driver/panther_robot_driver.cpp
+    src/robot_system/system_ros_interface.cpp
+    src/robot_system/panther_system.cpp
+    src/robot_system/ugv_system.cpp
     src/utils.cpp)
   set_tests_properties(${PROJECT_NAME}_test_panther_system PROPERTIES TIMEOUT
                                                                       120)
@@ -323,7 +324,7 @@ if(BUILD_TESTING)
     hardware_interface
     rclcpp
     panther_msgs
-    panther_utils
+    husarion_ugv_utils
     std_msgs
     std_srvs)
   target_link_libraries(${PROJECT_NAME}_test_panther_system
@@ -332,8 +333,9 @@ if(BUILD_TESTING)
   # Integration tests option(TEST_INTEGRATION "Run integration tests" OFF)
   if(TEST_INTEGRATION) # Hardware integration
     ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver
-                    test/panther_system/gpio/test_gpio_driver.cpp)
-    ament_target_dependencies(${PROJECT_NAME}_test_gpiod_driver panther_utils)
+                    test/robot_system/gpio/test_gpio_driver.cpp)
+    ament_target_dependencies(${PROJECT_NAME}_test_gpiod_driver
+                              husarion_ugv_utils)
     target_link_libraries(${PROJECT_NAME}_test_gpiod_driver ${PROJECT_NAME}
                           PkgConfig::LIBGPIOD)
   endif()
diff --git a/panther_hardware_interfaces/CODE_STRUCTURE.md b/husarion_ugv_hardware_interfaces/CODE_STRUCTURE.md
similarity index 88%
rename from panther_hardware_interfaces/CODE_STRUCTURE.md
rename to husarion_ugv_hardware_interfaces/CODE_STRUCTURE.md
index d7a448d4..0b6a7b59 100644
--- a/panther_hardware_interfaces/CODE_STRUCTURE.md
+++ b/husarion_ugv_hardware_interfaces/CODE_STRUCTURE.md
@@ -1,6 +1,6 @@
 # Structure
 
-A brief introduction to the code structure of the Panther system.
+A brief introduction to the code structure of the Husarion UGV system.
 
 ## RoboteqDriver
 
@@ -44,7 +44,7 @@ As they usually are rare and singular occurrences, it is better to filter some o
 ## GPIODriver
 
 The GPIODriver is a low-level class responsible for direct interaction with the GPIO (General Purpose Input/Output) pins on the Raspberry Pi.
-It comprises a wrapper implementation for the GPIOD library, enabling real-time manipulation of GPIO pins on the Raspberry Pi. Offering convenient interfaces for setting pin values, altering their direction, monitoring events, and conducting other GPIO operations, this library facilitates effective GPIO pin management on the Panther robot. It simplifies integration within robotic applications.
+It comprises a wrapper implementation for the GPIOD library, enabling real-time manipulation of GPIO pins on the Raspberry Pi. Offering convenient interfaces for setting pin values, altering their direction, monitoring events, and conducting other GPIO operations, this library facilitates effective GPIO pin management on the Husarion UGV. It simplifies integration within robotic applications.
 
 ## GPIOController
 
@@ -60,14 +60,13 @@ The GPIOController provides wrappers for the GPIO driver, handling reading and w
 Implementation of emergency stop handling.
 
 * `EStopInterface`: Interface for versioned emergency stop implementations.
-* `EStopPTH12X`: Class with specific logic for the Panther robot with version 1.20 and above.
-* `EStopPTH10X`: Class with specific logic for the Panther robot with version below 1.20.
+* `EStop`: Class with specific logic for the Husarion UGV.
 
-## PantherSystemRosInterface
+## SystemRosInterface
 
-A class that takes care of additional ROS interface of panther system, such as publishing driver state and providing service for clearing errors.
+A class that takes care of additional ROS interface of Husarion UGV system, such as publishing driver state and providing service for clearing errors.
 
-## PantherSystem
+## {Robot}System
 
 The main class that implements SystemInterface from ros2_control (for details refer to the [ros2_control documentation](https://control.ros.org/master/index.html)).
 Handles transitions (initialization, activation, shutdown, error, etc.), provides interfaces for feedback (position, velocity, effort) and commands (velocity).
diff --git a/panther_hardware_interfaces/README.md b/husarion_ugv_hardware_interfaces/README.md
similarity index 95%
rename from panther_hardware_interfaces/README.md
rename to husarion_ugv_hardware_interfaces/README.md
index a7f49b4f..980b9367 100644
--- a/panther_hardware_interfaces/README.md
+++ b/husarion_ugv_hardware_interfaces/README.md
@@ -1,10 +1,10 @@
-# panther_hardware_interfaces
+# husarion_ugv_hardware_interfaces
 
-Package that implements SystemInterface from ros2_control for Panther.
+Package that implements SystemInterface from ros2_control for Husarion UGV.
 
 ## ROS Nodes
 
-This package doesn't contain any standalone nodes, only plugins that are loaded by the resource manager. To use this hardware interface, add it to your URDF. You can check how to do it in [panther_description](https://github.com/husarion/panther_ros/tree/ros2/panther_description/).
+This package doesn't contain any standalone nodes, only plugins that are loaded by the resource manager. To use this hardware interface, add it to your URDF. You can check how to do it in [lynx_description](https://github.com/husarion/panther_ros/tree/ros2/panther_description/) or [panther_description](https://github.com/husarion/panther_ros/tree/ros2/panther_description/).
 
 ### UGVSystem (PantherSystem | LynxSystem)
 
@@ -66,7 +66,7 @@ LynxSystem additional CAN settings
 > [!CAUTION]
 > `max_write_pdo_cmds_errors_count`, `max_read_pdo_motor_states_errors_count`, `max_read_pdo_driver_state_errors_count`, `sdo_operation_timeout`, `pdo_motor_states_timeout_ms` and `pdo_driver_state_timeout_ms` are safety-critical parameters, they should be changed only in very specific cases, be sure that you know how they work and be really cautious when changing them.
 
-### PantherImuSensor
+### PhidgetImuSensor
 
 Plugin responsible for communicating with IMU and filtering data using Madgwick Filter.
 
@@ -158,8 +158,8 @@ sudo apt install -y kmod iproute2
 ### Running tests
 
 ```bash
-colcon build --packages-select panther_hardware_interfaces --symlink-install
-colcon test --event-handlers console_direct+ --packages-select panther_hardware_interfaces --parallel-workers 1
+colcon build --packages-select husarion_ugv_hardware_interfaces --symlink-install
+colcon test --event-handlers console_direct+ --packages-select husarion_ugv_hardware_interfaces --parallel-workers 1
 colcon test-result --verbose --all
 ```
 
diff --git a/panther_hardware_interfaces/cmake/SuperBuild.cmake b/husarion_ugv_hardware_interfaces/cmake/SuperBuild.cmake
similarity index 98%
rename from panther_hardware_interfaces/cmake/SuperBuild.cmake
rename to husarion_ugv_hardware_interfaces/cmake/SuperBuild.cmake
index c7fca72e..685b8d1b 100644
--- a/panther_hardware_interfaces/cmake/SuperBuild.cmake
+++ b/husarion_ugv_hardware_interfaces/cmake/SuperBuild.cmake
@@ -46,7 +46,7 @@ ExternalProject_Add(
   BUILD_IN_SOURCE 1)
 
 ExternalProject_Add(
-  ep_panther_hardware_interfaces
+  ep_husarion_ugv_hardware_interfaces
   DEPENDS ${DEPENDENCIES}
   SOURCE_DIR ${PROJECT_SOURCE_DIR}
   CMAKE_ARGS -DUSE_SUPERBUILD=OFF
diff --git a/panther_hardware_interfaces/config/canopen_configuration.yaml b/husarion_ugv_hardware_interfaces/config/canopen_configuration.yaml
similarity index 100%
rename from panther_hardware_interfaces/config/canopen_configuration.yaml
rename to husarion_ugv_hardware_interfaces/config/canopen_configuration.yaml
diff --git a/panther_hardware_interfaces/config/master.dcf b/husarion_ugv_hardware_interfaces/config/master.dcf
similarity index 100%
rename from panther_hardware_interfaces/config/master.dcf
rename to husarion_ugv_hardware_interfaces/config/master.dcf
diff --git a/panther_hardware_interfaces/config/phidgets_spatial_parameters.yaml b/husarion_ugv_hardware_interfaces/config/phidgets_spatial_parameters.yaml
similarity index 100%
rename from panther_hardware_interfaces/config/phidgets_spatial_parameters.yaml
rename to husarion_ugv_hardware_interfaces/config/phidgets_spatial_parameters.yaml
diff --git a/panther_hardware_interfaces/config/roboteq_motor_controllers_v80_21a.eds b/husarion_ugv_hardware_interfaces/config/roboteq_motor_controllers_v80_21a.eds
similarity index 100%
rename from panther_hardware_interfaces/config/roboteq_motor_controllers_v80_21a.eds
rename to husarion_ugv_hardware_interfaces/config/roboteq_motor_controllers_v80_21a.eds
diff --git a/husarion_ugv_hardware_interfaces/husarion_ugv_hardware_interfaces.xml b/husarion_ugv_hardware_interfaces/husarion_ugv_hardware_interfaces.xml
new file mode 100644
index 00000000..47bd30d6
--- /dev/null
+++ b/husarion_ugv_hardware_interfaces/husarion_ugv_hardware_interfaces.xml
@@ -0,0 +1,25 @@
+<library path="husarion_ugv_hardware_interfaces">
+  <class name="husarion_ugv_hardware_interfaces/PantherSystem"
+    type="husarion_ugv_hardware_interfaces::PantherSystem"
+    base_class_type="hardware_interface::SystemInterface">
+    <description>
+      Hardware system controller for Panther's Roboteq motor controller
+    </description>
+  </class>
+
+  <class name="husarion_ugv_hardware_interfaces/LynxSystem"
+    type="husarion_ugv_hardware_interfaces::LynxSystem"
+    base_class_type="hardware_interface::SystemInterface">
+    <description>
+      Hardware system controller for Lynx's Roboteq motor controller
+    </description>
+  </class>
+
+  <class name="husarion_ugv_hardware_interfaces/PhidgetImuSensor"
+    type="husarion_ugv_hardware_interfaces::PhidgetImuSensor"
+    base_class_type="hardware_interface::SensorInterface">
+    <description>
+      Hardware IMU sensor for Phidget Spatial IMU
+    </description>
+  </class>
+</library>
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_imu_sensor/panther_imu_sensor.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/phidget_imu_sensor/phidget_imu_sensor.hpp
similarity index 91%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_imu_sensor/panther_imu_sensor.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/phidget_imu_sensor/phidget_imu_sensor.hpp
index 5ac14ee4..ea3942f5 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_imu_sensor/panther_imu_sensor.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/phidget_imu_sensor/phidget_imu_sensor.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_IMU_SENSOR_PANTHER_IMU_SENSOR_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_IMU_SENSOR_PANTHER_IMU_SENSOR_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_PHIDGET_IMU_SENSOR_PHIDGET_IMU_SENSOR_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_PHIDGET_IMU_SENSOR_PHIDGET_IMU_SENSOR_HPP_
 
 #include <array>
 #include <condition_variable>
@@ -43,7 +43,7 @@
 
 using namespace std::placeholders;
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 using return_type = hardware_interface::return_type;
@@ -52,12 +52,12 @@ using StateInterface = hardware_interface::StateInterface;
 using CommandInterface = hardware_interface::CommandInterface;
 
 /**
- * @brief Class that implements SensorInterface from ros2_control for Panther
+ * @brief Class that implements SensorInterface from ros2_control for PhidgetSpatial IMU
  */
-class PantherImuSensor : public hardware_interface::SensorInterface
+class PhidgetImuSensor : public hardware_interface::SensorInterface
 {
 public:
-  RCLCPP_SHARED_PTR_DEFINITIONS(PantherImuSensor)
+  RCLCPP_SHARED_PTR_DEFINITIONS(PhidgetImuSensor)
 
   CallbackReturn on_init(const hardware_interface::HardwareInfo & hardware_info) override;
   CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
@@ -143,7 +143,7 @@ class PantherImuSensor : public hardware_interface::SensorInterface
 
   std::vector<double> imu_sensor_state_;
 
-  rclcpp::Logger logger_{rclcpp::get_logger("PantherImuSensor")};
+  rclcpp::Logger logger_{rclcpp::get_logger("PhidgetImuSensor")};
   rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
 
   inline static const std::array<std::string, kImuInterfacesSize> kImuInterfacesNames = {
@@ -180,6 +180,6 @@ class PantherImuSensor : public hardware_interface::SensorInterface
   rclcpp::Time last_spatial_data_timestamp_;
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_IMU_SENSOR_PANTHER_IMU_SENSOR_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_PHIDGET_IMU_SENSOR_PHIDGET_IMU_SENSOR_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp
similarity index 95%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp
index 53315d8f..f563d250 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp
@@ -17,8 +17,8 @@
  * @brief Header file containing a higher-level wrapper for the GPIO driver.
  */
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_GPIO_CONTROLLER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_GPIO_CONTROLLER_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_GPIO_CONTROLLER_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_GPIO_CONTROLLER_HPP_
 
 #include <atomic>
 #include <functional>
@@ -29,12 +29,12 @@
 
 #include "gpiod.hpp"
 
-#include "panther_utils/common_utilities.hpp"
+#include "husarion_ugv_utils/common_utilities.hpp"
 
-#include "panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp"
-#include "panther_hardware_interfaces/panther_system/gpio/types.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/gpio/types.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 class EStopResetInterrupted : public std::exception
@@ -403,7 +403,7 @@ class GPIOControllerFactory
   {
     std::unique_ptr<GPIOControllerInterface> gpio_controller;
 
-    if (panther_utils::common_utilities::MeetsVersionRequirement(robot_version, 1.2)) {
+    if (husarion_ugv_utils::common_utilities::MeetsVersionRequirement(robot_version, 1.2)) {
       auto config_info_storage = GPIOControllerPTH12X::GetGPIOConfigInfoStorage();
       auto gpio_driver = std::make_shared<GPIODriver>(config_info_storage);
 
@@ -419,6 +419,6 @@ class GPIOControllerFactory
   };
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_GPIO_CONTROLLER_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_GPIO_CONTROLLER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp
similarity index 95%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp
index e46775fc..a79b29f9 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_GPIO_DRIVER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_GPIO_DRIVER_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_GPIO_DRIVER_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_GPIO_DRIVER_HPP_
 
 #include <atomic>
 #include <condition_variable>
@@ -29,9 +29,9 @@
 
 #include "gpiod.hpp"
 
-#include "panther_hardware_interfaces/panther_system/gpio/types.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/gpio/types.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 class GPIODriverInterface
@@ -247,6 +247,6 @@ class GPIODriver : public GPIODriverInterface
   const std::filesystem::path gpio_chip_path_ = "/dev/gpiochip0";
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_GPIO_DRIVER_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_GPIO_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/types.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/types.hpp
similarity index 87%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/types.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/types.hpp
index 9695a8fd..987380ba 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/types.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/types.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_TYPES_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_TYPES_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_TYPES_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_TYPES_HPP_
 
 #include <map>
 #include <string>
 
 #include "gpiod.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 /**
@@ -83,6 +83,6 @@ struct GPIOInfo
   gpiod::line::offset offset = -1;
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_TYPES_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_TYPES_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/lynx_system.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/lynx_system.hpp
similarity index 80%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/lynx_system.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/lynx_system.hpp
index b5927e59..40a61823 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/lynx_system.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/lynx_system.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_LYNX_SYSTEM_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_LYNX_SYSTEM_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_LYNX_SYSTEM_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_LYNX_SYSTEM_HPP_
 
 #include <string>
 #include <vector>
 
-#include "panther_hardware_interfaces/panther_system/ugv_system.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 /**
@@ -55,6 +55,6 @@ class LynxSystem : public UGVSystem
   static const inline std::vector<std::string> kJointOrder = {"fl", "fr", "rl", "rr"};
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_LYNX_SYSTEM_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_LYNX_SYSTEM_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/panther_system.hpp
similarity index 80%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/panther_system.hpp
index 80cf06bb..acd1217e 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/panther_system.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_PANTHER_SYSTEM_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_PANTHER_SYSTEM_HPP_
 
 #include <string>
 #include <vector>
 
-#include "panther_hardware_interfaces/panther_system/ugv_system.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 /**
@@ -55,6 +55,6 @@ class PantherSystem : public UGVSystem
   static const inline std::vector<std::string> kJointOrder = {"fl", "fr", "rl", "rr"};
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_PANTHER_SYSTEM_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp
similarity index 91%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp
index 2c00eb69..8ca1fbe0 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_CANOPEN_MANAGER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_CANOPEN_MANAGER_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_CANOPEN_MANAGER_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_CANOPEN_MANAGER_HPP_
 
 #include <atomic>
 #include <chrono>
@@ -33,7 +33,7 @@
 #include "lely/io2/sys/sigset.hpp"
 #include "lely/io2/sys/timer.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 struct CANopenObject
@@ -133,6 +133,6 @@ class CANopenManager
   const CANopenSettings canopen_settings_;
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_CANOPEN_MANAGER_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_CANOPEN_MANAGER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/driver.hpp
similarity index 91%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/driver.hpp
index e4cc87c5..760cfde5 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/driver.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/driver.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_DRIVER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_DRIVER_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_DRIVER_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_DRIVER_HPP_
 
 #include <atomic>
 #include <chrono>
@@ -25,7 +25,7 @@
 
 #include "lely/coapp/loop_driver.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 struct MotorDriverState
@@ -145,6 +145,6 @@ class DriverInterface
   using SharedPtr = std::shared_ptr<DriverInterface>;
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_DRIVER_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/lynx_robot_driver.hpp
similarity index 76%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/lynx_robot_driver.hpp
index 25b61114..4991373a 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/lynx_robot_driver.hpp
@@ -12,18 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_LYNX_ROBOT_DRIVER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_LYNX_ROBOT_DRIVER_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_LYNX_ROBOT_DRIVER_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_LYNX_ROBOT_DRIVER_HPP_
 
 #include <chrono>
 #include <memory>
 #include <vector>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 /**
@@ -66,6 +66,6 @@ class LynxRobotDriver : public RoboteqRobotDriver
   void DefineDrivers() override;
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_LYNX_ROBOT_DRIVER_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_LYNX_ROBOT_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/panther_robot_driver.hpp
similarity index 77%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/panther_robot_driver.hpp
index 40665e76..d72d36f2 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/panther_robot_driver.hpp
@@ -12,18 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_PANTHER_ROBOT_DRIVER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_PANTHER_ROBOT_DRIVER_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_PANTHER_ROBOT_DRIVER_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_PANTHER_ROBOT_DRIVER_HPP_
 
 #include <chrono>
 #include <memory>
 #include <vector>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 /**
@@ -68,6 +68,6 @@ class PantherRobotDriver : public RoboteqRobotDriver
   virtual void DefineDrivers();
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_PANTHER_ROBOT_DRIVER_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_PANTHER_ROBOT_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp
similarity index 87%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp
index 78e38064..177e6142 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_
 
 #include <string>
 #include <vector>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 /**
@@ -116,6 +116,6 @@ class RobotDriverInterface
   virtual bool CommunicationError() = 0;
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp
similarity index 93%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp
index afc071e4..ba3649db 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DATA_CONVERTERS_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DATA_CONVERTERS_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DATA_CONVERTERS_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DATA_CONVERTERS_HPP_
 
 #include <algorithm>
 #include <bitset>
@@ -25,10 +25,10 @@
 #include "panther_msgs/msg/runtime_error.hpp"
 #include "panther_msgs/msg/script_flag.hpp"
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
-#include "panther_hardware_interfaces/utils.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/utils.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 struct DrivetrainSettings
@@ -258,6 +258,6 @@ class DriverData
   bool heartbeat_timeout_ = false;
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DATA_CONVERTERS_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DATA_CONVERTERS_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp
similarity index 93%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp
index a499120a..729339ab 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DRIVER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DRIVER_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DRIVER_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DRIVER_HPP_
 
 #include <atomic>
 #include <chrono>
@@ -25,9 +25,9 @@
 
 #include "lely/coapp/loop_driver.hpp"
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/driver.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 // Forward declaration
@@ -195,6 +195,6 @@ class RoboteqMotorDriver : public MotorDriverInterface
   const std::uint8_t channel_;
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DRIVER_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_error_filter.hpp
similarity index 90%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_error_filter.hpp
index 53a13d10..063df2da 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_error_filter.hpp
@@ -12,14 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ERROR_FILTER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ERROR_FILTER_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ERROR_FILTER_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ERROR_FILTER_HPP_
 
 #include <atomic>
 #include <map>
 #include <string>
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 class ErrorFilter
@@ -102,6 +102,6 @@ class RoboteqErrorFilter
   std::map<ErrorsFilterIds, ErrorFilter> error_filters_;
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ERROR_FILTER_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ERROR_FILTER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp
similarity index 87%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp
index 6f872307..ea088a56 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp
@@ -12,19 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ROBOT_DRIVER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ROBOT_DRIVER_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ROBOT_DRIVER_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ROBOT_DRIVER_HPP_
 
 #include <chrono>
 #include <string>
 #include <vector>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 struct MotorNames
@@ -170,6 +170,6 @@ class RoboteqRobotDriver : public RobotDriverInterface
   const std::chrono::milliseconds activate_wait_time_;
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ROBOT_DRIVER_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ROBOT_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_e_stop.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/system_e_stop.hpp
similarity index 87%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_e_stop.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/system_e_stop.hpp
index a0d1088e..7cc903a3 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_e_stop.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/system_e_stop.hpp
@@ -12,18 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_E_STOP_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_E_STOP_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_SYSTEM_E_STOP_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_SYSTEM_E_STOP_HPP_
 
 #include <atomic>
 #include <memory>
 #include <mutex>
 
-#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_error_filter.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 /**
@@ -115,6 +115,6 @@ class EStop : public EStopInterface
   std::atomic_bool e_stop_triggered_ = true;
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_E_STOP_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_SYSTEM_E_STOP_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_ros_interface.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/system_ros_interface.hpp
similarity index 96%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_ros_interface.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/system_ros_interface.hpp
index 856d4b72..4b813e17 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_ros_interface.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/system_ros_interface.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_
 
 #include <any>
 #include <functional>
@@ -34,12 +34,12 @@
 #include "panther_msgs/msg/io_state.hpp"
 #include "panther_msgs/msg/robot_driver_state.hpp"
 
-#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp"
 
 using namespace std::placeholders;
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 using BoolMsg = std_msgs::msg::Bool;
@@ -308,6 +308,6 @@ class SystemROSInterface
   std::vector<std::any> service_wrappers_storage_;
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/ugv_system.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp
similarity index 88%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/ugv_system.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp
index 03fd02b5..ac4e574c 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/ugv_system.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_UGV_SYSTEM_HPP_
-#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_UGV_SYSTEM_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_UGV_SYSTEM_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_UGV_SYSTEM_HPP_
 
 #include <functional>
 #include <memory>
@@ -29,14 +29,14 @@
 #include <hardware_interface/system_interface.hpp>
 #include <hardware_interface/types/hardware_interface_return_values.hpp>
 
-#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp"
-#include "panther_hardware_interfaces/panther_system/system_e_stop.hpp"
-#include "panther_hardware_interfaces/panther_system/system_ros_interface.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_error_filter.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/system_e_stop.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/system_ros_interface.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 using return_type = hardware_interface::return_type;
@@ -45,7 +45,7 @@ using StateInterface = hardware_interface::StateInterface;
 using CommandInterface = hardware_interface::CommandInterface;
 
 /**
- * @brief Class that implements SystemInterface from ros2_control for Husarion UGV robots
+ * @brief Class that implements SystemInterface from ros2_control for Husarion UGV
  */
 class UGVSystem : public hardware_interface::SystemInterface
 {
@@ -163,6 +163,6 @@ class UGVSystem : public hardware_interface::SystemInterface
   rclcpp::Duration driver_states_update_period_{0, 0};
 };
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_UGV_SYSTEM_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_UGV_SYSTEM_HPP_
diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/utils.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/utils.hpp
similarity index 89%
rename from panther_hardware_interfaces/include/panther_hardware_interfaces/utils.hpp
rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/utils.hpp
index 9a53b792..b64e8522 100644
--- a/panther_hardware_interfaces/include/panther_hardware_interfaces/utils.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/utils.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_UTILS_HPP_
-#define PANTHER_HARDWARE_INTERFACES_UTILS_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_UTILS_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_UTILS_HPP_
 
 #include <cstdint>
 #include <functional>
 #include <stdexcept>
 #include <string>
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 /**
@@ -59,6 +59,6 @@ bool OperationWithAttempts(
  */
 bool CheckIfJointNameContainValidSequence(const std::string & name, const std::string & sequence);
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
-#endif  // PANTHER_HARDWARE_INTERFACES_UTILS_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_UTILS_HPP_
diff --git a/panther_hardware_interfaces/package.xml b/husarion_ugv_hardware_interfaces/package.xml
similarity index 92%
rename from panther_hardware_interfaces/package.xml
rename to husarion_ugv_hardware_interfaces/package.xml
index f3c1dfee..90798d56 100644
--- a/panther_hardware_interfaces/package.xml
+++ b/husarion_ugv_hardware_interfaces/package.xml
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>panther_hardware_interfaces</name>
+  <name>husarion_ugv_hardware_interfaces</name>
   <version>2.1.1</version>
-  <description>Hardware controller for Panther</description>
+  <description>Hardware controller for Husarion UGV</description>
   <maintainer email="support@husarion.com">Husarion</maintainer>
   <license>Apache License 2.0</license>
 
@@ -31,9 +31,9 @@
   <depend>generate_parameter_library</depend>
   <depend>geometry_msgs</depend>
   <depend>hardware_interface</depend>
+  <depend>husarion_ugv_utils</depend>
   <depend>imu_filter_madgwick</depend>
   <depend>panther_msgs</depend>
-  <depend>panther_utils</depend>
   <depend>phidgets_api</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
diff --git a/panther_hardware_interfaces/src/panther_imu_sensor/panther_imu_sensor.cpp b/husarion_ugv_hardware_interfaces/src/phidget_imu_sensor/phidget_imu_sensor.cpp
similarity index 86%
rename from panther_hardware_interfaces/src/panther_imu_sensor/panther_imu_sensor.cpp
rename to husarion_ugv_hardware_interfaces/src/phidget_imu_sensor/phidget_imu_sensor.cpp
index 1cd14517..75f9405b 100644
--- a/panther_hardware_interfaces/src/panther_imu_sensor/panther_imu_sensor.cpp
+++ b/husarion_ugv_hardware_interfaces/src/phidget_imu_sensor/phidget_imu_sensor.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_imu_sensor/panther_imu_sensor.hpp"
+#include "husarion_ugv_hardware_interfaces/phidget_imu_sensor/phidget_imu_sensor.hpp"
 
 #include <array>
 #include <chrono>
@@ -30,10 +30,10 @@
 
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
-CallbackReturn PantherImuSensor::on_init(const hardware_interface::HardwareInfo & hardware_info)
+CallbackReturn PhidgetImuSensor::on_init(const hardware_interface::HardwareInfo & hardware_info)
 {
   if (hardware_interface::SensorInterface::on_init(hardware_info) != CallbackReturn::SUCCESS) {
     return CallbackReturn::ERROR;
@@ -52,7 +52,7 @@ CallbackReturn PantherImuSensor::on_init(const hardware_interface::HardwareInfo
   return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn PantherImuSensor::on_configure(const rclcpp_lifecycle::State &)
+CallbackReturn PhidgetImuSensor::on_configure(const rclcpp_lifecycle::State &)
 {
   try {
     ReadObligatoryParams();
@@ -91,12 +91,12 @@ CallbackReturn PantherImuSensor::on_configure(const rclcpp_lifecycle::State &)
   return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn PantherImuSensor::on_cleanup(const rclcpp_lifecycle::State &)
+CallbackReturn PhidgetImuSensor::on_cleanup(const rclcpp_lifecycle::State &)
 {
   return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn PantherImuSensor::on_activate(const rclcpp_lifecycle::State &)
+CallbackReturn PhidgetImuSensor::on_activate(const rclcpp_lifecycle::State &)
 {
   rclcpp::NodeOptions ros_interface_options;
 
@@ -108,9 +108,9 @@ CallbackReturn PantherImuSensor::on_activate(const rclcpp_lifecycle::State &)
     if (!spatial_) {
       spatial_ = std::make_unique<phidgets::Spatial>(
         params_.serial, params_.hub_port, false,
-        std::bind(&PantherImuSensor::SpatialDataCallback, this, _1, _2, _3, _4), nullptr,
-        std::bind(&PantherImuSensor::SpatialAttachCallback, this),
-        std::bind(&PantherImuSensor::SpatialDetachCallback, this));
+        std::bind(&PhidgetImuSensor::SpatialDataCallback, this, _1, _2, _3, _4), nullptr,
+        std::bind(&PhidgetImuSensor::SpatialAttachCallback, this),
+        std::bind(&PhidgetImuSensor::SpatialDetachCallback, this));
     }
 
     imu_connected_ = true;
@@ -131,22 +131,22 @@ CallbackReturn PantherImuSensor::on_activate(const rclcpp_lifecycle::State &)
   return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn PantherImuSensor::on_deactivate(const rclcpp_lifecycle::State &)
+CallbackReturn PhidgetImuSensor::on_deactivate(const rclcpp_lifecycle::State &)
 {
   return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn PantherImuSensor::on_shutdown(const rclcpp_lifecycle::State &)
+CallbackReturn PhidgetImuSensor::on_shutdown(const rclcpp_lifecycle::State &)
 {
   return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn PantherImuSensor::on_error(const rclcpp_lifecycle::State &)
+CallbackReturn PhidgetImuSensor::on_error(const rclcpp_lifecycle::State &)
 {
   return CallbackReturn::SUCCESS;
 }
 
-std::vector<StateInterface> PantherImuSensor::export_state_interfaces()
+std::vector<StateInterface> PhidgetImuSensor::export_state_interfaces()
 {
   std::vector<StateInterface> state_interfaces;
 
@@ -159,7 +159,7 @@ std::vector<StateInterface> PantherImuSensor::export_state_interfaces()
   return state_interfaces;
 }
 
-return_type PantherImuSensor::read(
+return_type PhidgetImuSensor::read(
   const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
 {
   if (!imu_connected_) {
@@ -168,14 +168,14 @@ return_type PantherImuSensor::read(
   return return_type::OK;
 }
 
-void PantherImuSensor::CheckSensorName() const
+void PhidgetImuSensor::CheckSensorName() const
 {
   if (!info_.sensors.size()) {
     throw std::runtime_error("Sensor is not defined in URDF!");
   }
 }
 
-void PantherImuSensor::CheckStatesSize() const
+void PhidgetImuSensor::CheckStatesSize() const
 {
   if (info_.sensors.at(0).state_interfaces.size() != kImuInterfacesSize) {
     throw std::runtime_error(
@@ -185,7 +185,7 @@ void PantherImuSensor::CheckStatesSize() const
   }
 }
 
-void PantherImuSensor::CheckInterfaces() const
+void PhidgetImuSensor::CheckInterfaces() const
 {
   const auto names_start_iter = kImuInterfacesNames.begin();
   const auto names_end_iter = kImuInterfacesNames.end();
@@ -201,7 +201,7 @@ void PantherImuSensor::CheckInterfaces() const
   }
 }
 
-void PantherImuSensor::ReadObligatoryParams()
+void PhidgetImuSensor::ReadObligatoryParams()
 {
   params_.serial = std::stoi(info_.hardware_parameters.at("serial"));
   params_.hub_port = std::stoi(info_.hardware_parameters.at("hub_port"));
@@ -215,7 +215,7 @@ void PantherImuSensor::ReadObligatoryParams()
   }
 }
 
-void PantherImuSensor::ReadCompassParams()
+void PhidgetImuSensor::ReadCompassParams()
 {
   params_.cc_mag_field = hardware_interface::stod(info_.hardware_parameters.at("cc_mag_field"));
   params_.cc_offset0 = hardware_interface::stod(info_.hardware_parameters.at("cc_offset0"));
@@ -232,7 +232,7 @@ void PantherImuSensor::ReadCompassParams()
   params_.cc_t5 = hardware_interface::stod(info_.hardware_parameters.at("cc_t5"));
 }
 
-void PantherImuSensor::ReadMadgwickFilterParams()
+void PhidgetImuSensor::ReadMadgwickFilterParams()
 {
   params_.gain = hardware_interface::stod(info_.hardware_parameters.at("gain"));
   params_.zeta = hardware_interface::stod(info_.hardware_parameters.at("zeta"));
@@ -247,7 +247,7 @@ void PantherImuSensor::ReadMadgwickFilterParams()
   CheckMadgwickFilterWorldFrameParam();
 }
 
-void PantherImuSensor::CheckMadgwickFilterWorldFrameParam()
+void PhidgetImuSensor::CheckMadgwickFilterWorldFrameParam()
 {
   const auto world_frame = info_.hardware_parameters.at("world_frame");
 
@@ -265,13 +265,13 @@ void PantherImuSensor::CheckMadgwickFilterWorldFrameParam()
   }
 }
 
-void PantherImuSensor::SetInitialValues()
+void PhidgetImuSensor::SetInitialValues()
 {
   imu_sensor_state_.resize(
     info_.sensors.at(0).state_interfaces.size(), std::numeric_limits<double>::quiet_NaN());
 }
 
-void PantherImuSensor::Calibrate()
+void PhidgetImuSensor::Calibrate()
 {
   spatial_->zero();
 
@@ -284,12 +284,12 @@ void PantherImuSensor::Calibrate()
   RCLCPP_INFO(logger_, "IMU sensor calibration completed.");
 }
 
-bool PantherImuSensor::IsParamDefined(const std::string & param_name) const
+bool PhidgetImuSensor::IsParamDefined(const std::string & param_name) const
 {
   return info_.hardware_parameters.find(param_name) != info_.hardware_parameters.end();
 }
 
-bool PantherImuSensor::AreParamsDefined(const std::unordered_set<std::string> & params_names) const
+bool PhidgetImuSensor::AreParamsDefined(const std::unordered_set<std::string> & params_names) const
 {
   for (const auto & param_name : params_names) {
     if (!IsParamDefined(param_name)) {
@@ -299,7 +299,7 @@ bool PantherImuSensor::AreParamsDefined(const std::unordered_set<std::string> &
   return true;
 }
 
-void PantherImuSensor::ConfigureCompassParams()
+void PhidgetImuSensor::ConfigureCompassParams()
 {
   if (AreParamsDefined(
         {"cc_mag_field", "cc_offset0", "cc_offset1", "cc_offset2", "cc_gain0", "cc_gain1",
@@ -316,7 +316,7 @@ void PantherImuSensor::ConfigureCompassParams()
   }
 }
 
-void PantherImuSensor::ConfigureHeating()
+void PhidgetImuSensor::ConfigureHeating()
 {
   if (IsParamDefined("heating_enabled")) {
     params_.heating_enabled =
@@ -327,7 +327,7 @@ void PantherImuSensor::ConfigureHeating()
   }
 }
 
-void PantherImuSensor::ConfigureMadgwickFilter()
+void PhidgetImuSensor::ConfigureMadgwickFilter()
 {
   filter_ = std::make_unique<ImuFilter>();
   filter_->setWorldFrame(world_frame_);
@@ -335,7 +335,7 @@ void PantherImuSensor::ConfigureMadgwickFilter()
   filter_->setDriftBiasGain(params_.zeta);
 }
 
-geometry_msgs::msg::Vector3 PantherImuSensor::ParseMagnitude(const double magnetic_field[3])
+geometry_msgs::msg::Vector3 PhidgetImuSensor::ParseMagnitude(const double magnetic_field[3])
 {
   geometry_msgs::msg::Vector3 mag_fld;
 
@@ -352,7 +352,7 @@ geometry_msgs::msg::Vector3 PantherImuSensor::ParseMagnitude(const double magnet
   return mag_fld;
 }
 
-geometry_msgs::msg::Vector3 PantherImuSensor::ParseGyration(const double angular_rate[3])
+geometry_msgs::msg::Vector3 PhidgetImuSensor::ParseGyration(const double angular_rate[3])
 {
   geometry_msgs::msg::Vector3 ang_vel;
 
@@ -362,7 +362,7 @@ geometry_msgs::msg::Vector3 PantherImuSensor::ParseGyration(const double angular
   return ang_vel;
 }
 
-geometry_msgs::msg::Vector3 PantherImuSensor::ParseAcceleration(const double acceleration[3])
+geometry_msgs::msg::Vector3 PhidgetImuSensor::ParseAcceleration(const double acceleration[3])
 {
   geometry_msgs::msg::Vector3 lin_acc;
 
@@ -372,7 +372,7 @@ geometry_msgs::msg::Vector3 PantherImuSensor::ParseAcceleration(const double acc
   return lin_acc;
 }
 
-void PantherImuSensor::InitializeMadgwickAlgorithm(
+void PhidgetImuSensor::InitializeMadgwickAlgorithm(
   const geometry_msgs::msg::Vector3 & mag_compensated, const geometry_msgs::msg::Vector3 & lin_acc,
   const rclcpp::Time & timestamp)
 {
@@ -389,7 +389,7 @@ void PantherImuSensor::InitializeMadgwickAlgorithm(
   algorithm_initialized_ = true;
 }
 
-void PantherImuSensor::RestartMadgwickAlgorithm()
+void PhidgetImuSensor::RestartMadgwickAlgorithm()
 {
   if (!filter_) {
     return;
@@ -399,7 +399,7 @@ void PantherImuSensor::RestartMadgwickAlgorithm()
   filter_->setOrientation(restarted_value, restarted_value, restarted_value, restarted_value);
 }
 
-bool PantherImuSensor::IsIMUCalibrated(const geometry_msgs::msg::Vector3 & mag_compensated)
+bool PhidgetImuSensor::IsIMUCalibrated(const geometry_msgs::msg::Vector3 & mag_compensated)
 {
   if (imu_calibrated_) {
     return true;
@@ -409,18 +409,18 @@ bool PantherImuSensor::IsIMUCalibrated(const geometry_msgs::msg::Vector3 & mag_c
   return imu_calibrated_;
 }
 
-bool PantherImuSensor::IsVectorFinite(const geometry_msgs::msg::Vector3 & vec)
+bool PhidgetImuSensor::IsVectorFinite(const geometry_msgs::msg::Vector3 & vec)
 {
   return std::isfinite(vec.x) && std::isfinite(vec.y) && std::isfinite(vec.z);
 }
 
-bool PantherImuSensor::IsMagnitudeSynchronizedWithAccelerationAndGyration(
+bool PhidgetImuSensor::IsMagnitudeSynchronizedWithAccelerationAndGyration(
   const geometry_msgs::msg::Vector3 & mag_compensated)
 {
   return IsVectorFinite(mag_compensated);
 }
 
-void PantherImuSensor::SpatialDataCallback(
+void PhidgetImuSensor::SpatialDataCallback(
   const double acceleration[3], const double angular_rate[3], const double magnetic_field[3],
   const double timestamp)
 {
@@ -475,14 +475,14 @@ void PantherImuSensor::SpatialDataCallback(
   UpdateAllStatesValues(ang_vel, lin_acc);
 }
 
-void PantherImuSensor::SpatialAttachCallback()
+void PhidgetImuSensor::SpatialAttachCallback()
 {
   RCLCPP_INFO(logger_, "IMU sensor has successfully attached and is now connected.");
   imu_connected_ = true;
   on_activate(rclcpp_lifecycle::State{});
 }
 
-void PantherImuSensor::SpatialDetachCallback()
+void PhidgetImuSensor::SpatialDetachCallback()
 {
   RCLCPP_WARN(
     logger_,
@@ -495,7 +495,7 @@ void PantherImuSensor::SpatialDetachCallback()
   on_deactivate(rclcpp_lifecycle::State{});
 }
 
-void PantherImuSensor::UpdateMadgwickAlgorithm(
+void PhidgetImuSensor::UpdateMadgwickAlgorithm(
   const geometry_msgs::msg::Vector3 & ang_vel, const geometry_msgs::msg::Vector3 & lin_acc,
   const geometry_msgs::msg::Vector3 & mag_compensated, const double dt)
 {
@@ -504,7 +504,7 @@ void PantherImuSensor::UpdateMadgwickAlgorithm(
     mag_compensated.y, mag_compensated.z, dt);
 }
 
-void PantherImuSensor::UpdateMadgwickAlgorithmIMU(
+void PhidgetImuSensor::UpdateMadgwickAlgorithmIMU(
   const geometry_msgs::msg::Vector3 & ang_vel, const geometry_msgs::msg::Vector3 & lin_acc,
   const double dt)
 {
@@ -512,7 +512,7 @@ void PantherImuSensor::UpdateMadgwickAlgorithmIMU(
     ang_vel.x, ang_vel.y, ang_vel.z, lin_acc.x, lin_acc.y, lin_acc.z, dt);
 }
 
-void PantherImuSensor::UpdateAccelerationAndGyrationStateValues(
+void PhidgetImuSensor::UpdateAccelerationAndGyrationStateValues(
   const geometry_msgs::msg::Vector3 & ang_vel, const geometry_msgs::msg::Vector3 & lin_acc)
 {
   imu_sensor_state_[angular_velocity_x] = ang_vel.x;
@@ -532,7 +532,7 @@ void PantherImuSensor::UpdateAccelerationAndGyrationStateValues(
   }
 }
 
-void PantherImuSensor::UpdateAllStatesValues(
+void PhidgetImuSensor::UpdateAllStatesValues(
   const geometry_msgs::msg::Vector3 & ang_vel, const geometry_msgs::msg::Vector3 & lin_acc)
 {
   filter_->getOrientation(
@@ -542,14 +542,14 @@ void PantherImuSensor::UpdateAllStatesValues(
   UpdateAccelerationAndGyrationStateValues(ang_vel, lin_acc);
 }
 
-void PantherImuSensor::SetStateValuesToNans()
+void PhidgetImuSensor::SetStateValuesToNans()
 {
   std::fill(
     imu_sensor_state_.begin(), imu_sensor_state_.end(), std::numeric_limits<double>::quiet_NaN());
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
 #include "pluginlib/class_list_macros.hpp"
 PLUGINLIB_EXPORT_CLASS(
-  panther_hardware_interfaces::PantherImuSensor, hardware_interface::SensorInterface)
+  husarion_ugv_hardware_interfaces::PhidgetImuSensor, hardware_interface::SensorInterface)
diff --git a/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp
similarity index 96%
rename from panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp
rename to husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp
index 7bfbf24c..d2847ab7 100644
--- a/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp"
 
 #include <chrono>
 #include <functional>
@@ -24,10 +24,10 @@
 
 #include "gpiod.hpp"
 
-#include "panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp"
-#include "panther_hardware_interfaces/panther_system/gpio/types.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/gpio/types.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 Watchdog::Watchdog(std::shared_ptr<GPIODriverInterface> gpio_driver)
@@ -342,4 +342,4 @@ const std::vector<GPIOInfo> & GPIOControllerPTH10X::GetGPIOConfigInfoStorage()
   return gpio_config_info_storage_;
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/gpio/gpio_driver.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_driver.cpp
similarity index 95%
rename from panther_hardware_interfaces/src/panther_system/gpio/gpio_driver.cpp
rename to husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_driver.cpp
index 09b8c0fd..d386b68d 100644
--- a/panther_hardware_interfaces/src/panther_system/gpio/gpio_driver.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_driver.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp"
 
 #include <algorithm>
 #include <chrono>
@@ -29,9 +29,9 @@
 
 #include "gpiod.hpp"
 
-#include "panther_utils/configure_rt.hpp"
+#include "husarion_ugv_utils/configure_rt.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 GPIODriver::GPIODriver(std::vector<GPIOInfo> gpio_info_storage)
@@ -78,7 +78,7 @@ void GPIODriver::ConfigureEdgeEventCallback(const std::function<void(const GPIOI
 std::unique_ptr<gpiod::line_request> GPIODriver::CreateLineRequest(gpiod::chip & chip)
 {
   auto request_builder = chip.prepare_request();
-  request_builder.set_consumer("panther_hardware_interfaces");
+  request_builder.set_consumer("husarion_ugv_hardware_interfaces");
 
   for (GPIOInfo & gpio_info : gpio_info_storage_) {
     ConfigureLineRequest(chip, request_builder, gpio_info);
@@ -222,7 +222,7 @@ void GPIODriver::GPIOMonitorOn()
 void GPIODriver::MonitorAsyncEvents()
 {
   if (use_rt_) {
-    panther_utils::ConfigureRT(gpio_monit_thread_sched_priority_);
+    husarion_ugv_utils::ConfigureRT(gpio_monit_thread_sched_priority_);
   }
 
   auto edge_event_buffer = gpiod::edge_event_buffer(edge_event_buffer_size_);
@@ -308,4 +308,4 @@ GPIOPin GPIODriver::GetPinFromOffset(const gpiod::line::offset & offset) const
     "Pin with offset " + std::to_string(offset) + " not found in GPIO info storage.");
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/lynx_system.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/lynx_system.cpp
similarity index 92%
rename from panther_hardware_interfaces/src/panther_system/lynx_system.cpp
rename to husarion_ugv_hardware_interfaces/src/robot_system/lynx_system.cpp
index 8d491af5..169f8c59 100644
--- a/panther_hardware_interfaces/src/panther_system/lynx_system.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/lynx_system.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/lynx_system.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/lynx_system.hpp"
 
 #include <memory>
 #include <string>
@@ -21,11 +21,11 @@
 #include "diagnostic_updater/diagnostic_status_wrapper.hpp"
 #include "rclcpp/logging.hpp"
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/lynx_robot_driver.hpp"
 
-#include "panther_utils/diagnostics.hpp"
+#include "husarion_ugv_utils/diagnostics.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 void LynxSystem::ReadCANopenSettingsDriverCANIDs()
@@ -133,7 +133,7 @@ void LynxSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & st
     level = diagnostic_updater::DiagnosticStatusWrapper::ERROR;
     message = "Error detected.";
 
-    panther_utils::diagnostics::AddKeyValueIfTrue(
+    husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(
       status, driver_data.GetErrorMap(), "Driver error: ");
   }
 
@@ -141,7 +141,7 @@ void LynxSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & st
     level = diagnostic_updater::DiagnosticStatusWrapper::ERROR;
     message = "Error detected.";
 
-    panther_utils::diagnostics::AddKeyValueIfTrue(
+    husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(
       status, roboteq_error_filter_->GetErrorMap(), "", " error");
   }
 
@@ -170,7 +170,8 @@ std::vector<float> LynxSystem::GetSpeedCommands() const
     static_cast<float>(hw_commands_velocities_[0]), static_cast<float>(hw_commands_velocities_[1])};
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
 #include "pluginlib/class_list_macros.hpp"
-PLUGINLIB_EXPORT_CLASS(panther_hardware_interfaces::LynxSystem, hardware_interface::SystemInterface)
+PLUGINLIB_EXPORT_CLASS(
+  husarion_ugv_hardware_interfaces::LynxSystem, hardware_interface::SystemInterface)
diff --git a/panther_hardware_interfaces/src/panther_system/panther_system.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/panther_system.cpp
similarity index 93%
rename from panther_hardware_interfaces/src/panther_system/panther_system.cpp
rename to husarion_ugv_hardware_interfaces/src/robot_system/panther_system.cpp
index e87ded9b..18a29560 100644
--- a/panther_hardware_interfaces/src/panther_system/panther_system.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/panther_system.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/panther_system.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/panther_system.hpp"
 
 #include <memory>
 #include <string>
@@ -21,11 +21,11 @@
 #include "diagnostic_updater/diagnostic_status_wrapper.hpp"
 #include "rclcpp/logging.hpp"
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/panther_robot_driver.hpp"
 
-#include "panther_utils/diagnostics.hpp"
+#include "husarion_ugv_utils/diagnostics.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 void PantherSystem::ReadCANopenSettingsDriverCANIDs()
@@ -160,7 +160,7 @@ void PantherSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper &
     level = diagnostic_updater::DiagnosticStatusWrapper::ERROR;
     message = "Error detected.";
 
-    panther_utils::diagnostics::AddKeyValueIfTrue(
+    husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(
       status, front_driver_data.GetErrorMap(), "Front driver error: ");
   }
 
@@ -169,7 +169,7 @@ void PantherSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper &
     level = diagnostic_updater::DiagnosticStatusWrapper::ERROR;
     message = "Error detected.";
 
-    panther_utils::diagnostics::AddKeyValueIfTrue(
+    husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(
       status, rear_driver_data.GetErrorMap(), "Rear driver error: ");
   }
 
@@ -177,7 +177,7 @@ void PantherSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper &
     level = diagnostic_updater::DiagnosticStatusWrapper::ERROR;
     message = "Error detected.";
 
-    panther_utils::diagnostics::AddKeyValueIfTrue(
+    husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(
       status, roboteq_error_filter_->GetErrorMap(), "", " error");
   }
 
@@ -214,8 +214,8 @@ std::vector<float> PantherSystem::GetSpeedCommands() const
     static_cast<float>(hw_commands_velocities_[2]), static_cast<float>(hw_commands_velocities_[3])};
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
 
 #include "pluginlib/class_list_macros.hpp"
 PLUGINLIB_EXPORT_CLASS(
-  panther_hardware_interfaces::PantherSystem, hardware_interface::SystemInterface)
+  husarion_ugv_hardware_interfaces::PantherSystem, hardware_interface::SystemInterface)
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/canopen_manager.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/canopen_manager.cpp
similarity index 92%
rename from panther_hardware_interfaces/src/panther_system/robot_driver/canopen_manager.cpp
rename to husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/canopen_manager.cpp
index 37dc5db5..4b98a7f7 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/canopen_manager.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/canopen_manager.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp"
 
 #include <condition_variable>
 #include <filesystem>
@@ -24,9 +24,9 @@
 
 #include "ament_index_cpp/get_package_share_directory.hpp"
 
-#include "panther_utils/configure_rt.hpp"
+#include "husarion_ugv_utils/configure_rt.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 CANopenManager::CANopenManager(const CANopenSettings & canopen_settings)
@@ -43,7 +43,7 @@ void CANopenManager::Initialize()
   canopen_communication_started_.store(false);
 
   try {
-    panther_utils::ConfigureRT(kCANopenThreadSchedPriority);
+    husarion_ugv_utils::ConfigureRT(kCANopenThreadSchedPriority);
   } catch (const std::runtime_error & e) {
     std::cerr << "An exception occurred while configuring RT: " << e.what() << std::endl
               << "Continuing with regular thread settings (it may have a negative impact on the "
@@ -137,7 +137,7 @@ void CANopenManager::InitializeCANCommunication()
   // dcfgen canopen_configuration.yaml -r
   // dcfgen comes with lely, -r option tells to enable remote PDO mapping
   std::string master_dcf_path = std::filesystem::path(ament_index_cpp::get_package_share_directory(
-                                  "panther_hardware_interfaces")) /
+                                  "husarion_ugv_hardware_interfaces")) /
                                 "config" / "master.dcf";
 
   master_ = std::make_shared<lely::canopen::AsyncMaster>(
@@ -156,4 +156,4 @@ void CANopenManager::NotifyCANCommunicationStarted(const bool result)
   canopen_communication_started_cond_.notify_all();
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/lynx_robot_driver.cpp
similarity index 86%
rename from panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
rename to husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/lynx_robot_driver.cpp
index 54b655af..90abcfca 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/lynx_robot_driver.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/lynx_robot_driver.cpp
@@ -12,17 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/lynx_robot_driver.hpp"
 
 #include <memory>
 #include <stdexcept>
 #include <string>
 #include <vector>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 void LynxRobotDriver::SendSpeedCommands(const std::vector<float> & speeds)
@@ -65,4 +65,4 @@ void LynxRobotDriver::DefineDrivers()
   drivers_.emplace(DriverNames::DEFAULT, driver);
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/panther_robot_driver.cpp
similarity index 90%
rename from panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp
rename to husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/panther_robot_driver.cpp
index 7cbbb974..9fe70960 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/panther_robot_driver.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/panther_robot_driver.cpp
@@ -12,17 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/panther_robot_driver.hpp"
 
 #include <memory>
 #include <stdexcept>
 #include <string>
 #include <vector>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 void PantherRobotDriver::SendSpeedCommands(const std::vector<float> & speeds)
@@ -87,4 +87,4 @@ void PantherRobotDriver::DefineDrivers()
   drivers_.emplace(DriverNames::REAR, rear_driver);
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_data_converters.cpp
similarity index 95%
rename from panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp
rename to husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_data_converters.cpp
index 0382edb7..e68902e3 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_data_converters.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_data_converters.cpp
@@ -12,14 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp"
 
 #include <cmath>
 
-#include "panther_hardware_interfaces/utils.hpp"
-#include "panther_utils/common_utilities.hpp"
+#include "husarion_ugv_hardware_interfaces/utils.hpp"
+#include "husarion_ugv_utils/common_utilities.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 RoboteqVelocityCommandConverter::RoboteqVelocityCommandConverter(
@@ -264,10 +264,10 @@ std::map<std::string, bool> DriverData::GetFlagErrorMap() const
   flag_error_map.merge(fault_flags_.GetErrorMap());
   flag_error_map.merge(script_flags_.GetErrorMap());
 
-  auto channel_1_runtime_error_map = panther_utils::common_utilities::PrefixMapKeys(
+  auto channel_1_runtime_error_map = husarion_ugv_utils::common_utilities::PrefixMapKeys(
     channel_1_runtime_error_.GetErrorMap(), "channel_1_motor.");
 
-  auto channel_2_runtime_error_map = panther_utils::common_utilities::PrefixMapKeys(
+  auto channel_2_runtime_error_map = husarion_ugv_utils::common_utilities::PrefixMapKeys(
     channel_2_runtime_error_.GetErrorMap(), "channel_2_motor.");
 
   flag_error_map.merge(std::move(channel_1_runtime_error_map));
@@ -291,4 +291,4 @@ std::map<std::string, bool> DriverData::GetErrorMap() const
   return error_map;
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_driver.cpp
similarity index 96%
rename from panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp
rename to husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_driver.cpp
index 56bed72d..1e05d1fb 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_driver.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_driver.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp"
 
 #include <chrono>
 #include <cmath>
@@ -24,10 +24,10 @@
 #include <string>
 #include <system_error>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
-#include "panther_hardware_interfaces/utils.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp"
+#include "husarion_ugv_hardware_interfaces/utils.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 // All ids and sub ids were read directly from the eds file. Lely CANopen doesn't have the option
@@ -273,4 +273,4 @@ void RoboteqDriver::OnRpdoWrite(const std::uint16_t idx, const std::uint8_t subi
   }
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_error_filter.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_error_filter.cpp
similarity index 93%
rename from panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_error_filter.cpp
rename to husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_error_filter.cpp
index 1d15abeb..81063c31 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_error_filter.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_error_filter.cpp
@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_error_filter.hpp"
 
 #include <algorithm>
 #include <atomic>
 #include <map>
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 void ErrorFilter::UpdateError(const bool current_error)
@@ -91,4 +91,4 @@ void RoboteqErrorFilter::ClearErrorsIfFlagSet()
   }
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_robot_driver.cpp
similarity index 93%
rename from panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
rename to husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_robot_driver.cpp
index 9a3626c0..18a61d91 100644
--- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_robot_driver.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp"
 
 #include <chrono>
 #include <ctime>
@@ -24,12 +24,12 @@
 
 #include "lely/util/chrono.hpp"
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 RoboteqRobotDriver::RoboteqRobotDriver(
@@ -257,4 +257,4 @@ void RoboteqRobotDriver::BootDrivers()
   }
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/system_e_stop.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/system_e_stop.cpp
similarity index 94%
rename from panther_hardware_interfaces/src/panther_system/system_e_stop.cpp
rename to husarion_ugv_hardware_interfaces/src/robot_system/system_e_stop.cpp
index 35efc987..969b3956 100644
--- a/panther_hardware_interfaces/src/panther_system/system_e_stop.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/system_e_stop.cpp
@@ -17,9 +17,9 @@
 #include <memory>
 #include <string>
 
-#include "panther_hardware_interfaces/panther_system/system_e_stop.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/system_e_stop.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 bool EStop::ReadEStopState()
@@ -83,4 +83,4 @@ void EStop::ResetEStop()
   }
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/system_ros_interface.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/system_ros_interface.cpp
similarity index 97%
rename from panther_hardware_interfaces/src/panther_system/system_ros_interface.cpp
rename to husarion_ugv_hardware_interfaces/src/robot_system/system_ros_interface.cpp
index f6b5564e..560789d2 100644
--- a/panther_hardware_interfaces/src/panther_system/system_ros_interface.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/system_ros_interface.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/system_ros_interface.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/system_ros_interface.hpp"
 
 #include <memory>
 #include <string>
@@ -22,9 +22,9 @@
 #include "rclcpp/rclcpp.hpp"
 #include "realtime_tools/realtime_publisher.h"
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 template class ROSServiceWrapper<std_srvs::srv::SetBool, std::function<void(bool)>>;
@@ -302,4 +302,4 @@ DriverStateNamedMsg & SystemROSInterface::GetDriverStateByName(
   return *it;
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/panther_system/ugv_system.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/ugv_system.cpp
similarity index 97%
rename from panther_hardware_interfaces/src/panther_system/ugv_system.cpp
rename to husarion_ugv_hardware_interfaces/src/robot_system/ugv_system.cpp
index fcdf497a..8f24a571 100644
--- a/panther_hardware_interfaces/src/panther_system/ugv_system.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/ugv_system.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/panther_system/ugv_system.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp"
 
 #include <array>
 #include <chrono>
@@ -27,14 +27,14 @@
 #include <hardware_interface/types/hardware_interface_type_values.hpp>
 #include <rclcpp/logging.hpp>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp"
-#include "panther_hardware_interfaces/panther_system/system_ros_interface.hpp"
-#include "panther_hardware_interfaces/utils.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/panther_robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/system_ros_interface.hpp"
+#include "husarion_ugv_hardware_interfaces/utils.hpp"
 
-#include "panther_utils/common_utilities.hpp"
-#include "panther_utils/diagnostics.hpp"
+#include "husarion_ugv_utils/common_utilities.hpp"
+#include "husarion_ugv_utils/diagnostics.hpp"
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 CallbackReturn UGVSystem::on_init(const hardware_interface::HardwareInfo & hardware_info)
@@ -547,4 +547,4 @@ void UGVSystem::MotorsPowerEnable(const bool enable)
   }
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
diff --git a/panther_hardware_interfaces/src/utils.cpp b/husarion_ugv_hardware_interfaces/src/utils.cpp
similarity index 93%
rename from panther_hardware_interfaces/src/utils.cpp
rename to husarion_ugv_hardware_interfaces/src/utils.cpp
index 1620a06b..a83d3967 100644
--- a/panther_hardware_interfaces/src/utils.cpp
+++ b/husarion_ugv_hardware_interfaces/src/utils.cpp
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_hardware_interfaces/utils.hpp"
+#include "husarion_ugv_hardware_interfaces/utils.hpp"
 
 #include <iostream>
 #include <stdexcept>
 
-namespace panther_hardware_interfaces
+namespace husarion_ugv_hardware_interfaces
 {
 
 bool OperationWithAttempts(
@@ -65,4 +65,4 @@ bool CheckIfJointNameContainValidSequence(const std::string & name, const std::s
   return true;
 }
 
-}  // namespace panther_hardware_interfaces
+}  // namespace husarion_ugv_hardware_interfaces
diff --git a/panther_hardware_interfaces/test/config/canopen_configuration.yaml b/husarion_ugv_hardware_interfaces/test/config/canopen_configuration.yaml
similarity index 100%
rename from panther_hardware_interfaces/test/config/canopen_configuration.yaml
rename to husarion_ugv_hardware_interfaces/test/config/canopen_configuration.yaml
diff --git a/panther_hardware_interfaces/test/config/slave_1.bin b/husarion_ugv_hardware_interfaces/test/config/slave_1.bin
similarity index 100%
rename from panther_hardware_interfaces/test/config/slave_1.bin
rename to husarion_ugv_hardware_interfaces/test/config/slave_1.bin
diff --git a/panther_hardware_interfaces/test/panther_imu_sensor/test_panther_imu_sensor.cpp b/husarion_ugv_hardware_interfaces/test/phidget_imu_sensor/test_phidget_imu_sensor.cpp
similarity index 73%
rename from panther_hardware_interfaces/test/panther_imu_sensor/test_panther_imu_sensor.cpp
rename to husarion_ugv_hardware_interfaces/test/phidget_imu_sensor/test_phidget_imu_sensor.cpp
index 9b99f98a..d7dad3fe 100644
--- a/panther_hardware_interfaces/test/panther_imu_sensor/test_panther_imu_sensor.cpp
+++ b/husarion_ugv_hardware_interfaces/test/phidget_imu_sensor/test_phidget_imu_sensor.cpp
@@ -26,67 +26,67 @@
 
 #include "lifecycle_msgs/msg/state.hpp"
 
-#include "panther_hardware_interfaces/panther_imu_sensor/panther_imu_sensor.hpp"
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_hardware_interfaces/phidget_imu_sensor/phidget_imu_sensor.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
-class PantherImuSensorWrapper : public panther_hardware_interfaces::PantherImuSensor
+class PhidgetImuSensorWrapper : public husarion_ugv_hardware_interfaces::PhidgetImuSensor
 {
 public:
-  PantherImuSensorWrapper() {}
+  PhidgetImuSensorWrapper() {}
 
   void SetHardwareInfo(const hardware_interface::HardwareInfo & info)
   {
     hardware_interface::SensorInterface::info_ = info;
   }
 
-  void CheckSensorName() const { PantherImuSensor::CheckSensorName(); }
+  void CheckSensorName() const { PhidgetImuSensor::CheckSensorName(); }
 
-  void CheckStatesSize() const { PantherImuSensor::CheckStatesSize(); }
+  void CheckStatesSize() const { PhidgetImuSensor::CheckStatesSize(); }
 
-  void CheckInterfaces() const { PantherImuSensor::CheckInterfaces(); }
+  void CheckInterfaces() const { PhidgetImuSensor::CheckInterfaces(); }
 
-  void ReadObligatoryParams() { PantherImuSensor::ReadObligatoryParams(); }
+  void ReadObligatoryParams() { PhidgetImuSensor::ReadObligatoryParams(); }
 
-  void ReadMadgwickFilterParams() { PantherImuSensor::ReadMadgwickFilterParams(); }
+  void ReadMadgwickFilterParams() { PhidgetImuSensor::ReadMadgwickFilterParams(); }
 
-  void ConfigureMadgwickFilter() { PantherImuSensor::ConfigureMadgwickFilter(); }
+  void ConfigureMadgwickFilter() { PhidgetImuSensor::ConfigureMadgwickFilter(); }
 
   void InitializeMadgwickAlgorithm(
     const geometry_msgs::msg::Vector3 & mag_compensated,
     const geometry_msgs::msg::Vector3 & lin_acc, const rclcpp::Time timestamp)
   {
-    PantherImuSensor::InitializeMadgwickAlgorithm(mag_compensated, lin_acc, timestamp);
+    PhidgetImuSensor::InitializeMadgwickAlgorithm(mag_compensated, lin_acc, timestamp);
   }
 
   void SpatialDataCallback(
     const double acceleration[3], const double angular_rate[3], const double magnetic_field[3],
     const double timestamp)
   {
-    PantherImuSensor::SpatialDataCallback(acceleration, angular_rate, magnetic_field, timestamp);
+    PhidgetImuSensor::SpatialDataCallback(acceleration, angular_rate, magnetic_field, timestamp);
   }
 
-  void SpatialAttachCallback() { PantherImuSensor::SpatialAttachCallback(); }
+  void SpatialAttachCallback() { PhidgetImuSensor::SpatialAttachCallback(); }
 
-  void SpatialDetachCallback() { PantherImuSensor::SpatialDetachCallback(); }
+  void SpatialDetachCallback() { PhidgetImuSensor::SpatialDetachCallback(); }
 
   geometry_msgs::msg::Vector3 ParseMagnitude(const double magnetic_field[3])
   {
-    return PantherImuSensor::ParseMagnitude(magnetic_field);
+    return PhidgetImuSensor::ParseMagnitude(magnetic_field);
   }
 
   geometry_msgs::msg::Vector3 ParseGyration(const double angular_rate[3])
   {
-    return PantherImuSensor::ParseGyration(angular_rate);
+    return PhidgetImuSensor::ParseGyration(angular_rate);
   }
 
   geometry_msgs::msg::Vector3 ParseAcceleration(const double acceleration[3])
   {
-    return PantherImuSensor::ParseAcceleration(acceleration);
+    return PhidgetImuSensor::ParseAcceleration(acceleration);
   }
 
   bool IsVectorFinite(const geometry_msgs::msg::Vector3 & vec)
   {
-    return PantherImuSensor::IsVectorFinite(vec);
+    return PhidgetImuSensor::IsVectorFinite(vec);
   }
 
   bool IsQuaternionFinite(const geometry_msgs::msg::Quaternion & quat)
@@ -97,55 +97,55 @@ class PantherImuSensorWrapper : public panther_hardware_interfaces::PantherImuSe
 
   bool IsIMUCalibrated(const geometry_msgs::msg::Vector3 & vec)
   {
-    return PantherImuSensor::IsIMUCalibrated(vec);
+    return PhidgetImuSensor::IsIMUCalibrated(vec);
   }
 
   geometry_msgs::msg::Quaternion GetQuaternion() const
   {
     geometry_msgs::msg::Quaternion q;
-    q.x = imu_sensor_state_[PantherImuSensor::orientation_x];
-    q.y = imu_sensor_state_[PantherImuSensor::orientation_y];
-    q.z = imu_sensor_state_[PantherImuSensor::orientation_z];
-    q.w = imu_sensor_state_[PantherImuSensor::orientation_w];
+    q.x = imu_sensor_state_[PhidgetImuSensor::orientation_x];
+    q.y = imu_sensor_state_[PhidgetImuSensor::orientation_y];
+    q.z = imu_sensor_state_[PhidgetImuSensor::orientation_z];
+    q.w = imu_sensor_state_[PhidgetImuSensor::orientation_w];
     return q;
   }
 
   geometry_msgs::msg::Vector3 GetAcceleration() const
   {
     geometry_msgs::msg::Vector3 accel;
-    accel.x = imu_sensor_state_[PantherImuSensor::linear_acceleration_x];
-    accel.y = imu_sensor_state_[PantherImuSensor::linear_acceleration_y];
-    accel.z = imu_sensor_state_[PantherImuSensor::linear_acceleration_z];
+    accel.x = imu_sensor_state_[PhidgetImuSensor::linear_acceleration_x];
+    accel.y = imu_sensor_state_[PhidgetImuSensor::linear_acceleration_y];
+    accel.z = imu_sensor_state_[PhidgetImuSensor::linear_acceleration_z];
     return accel;
   }
 
   geometry_msgs::msg::Vector3 GetGyration() const
   {
     geometry_msgs::msg::Vector3 gyro;
-    gyro.x = imu_sensor_state_[PantherImuSensor::angular_velocity_x];
-    gyro.y = imu_sensor_state_[PantherImuSensor::angular_velocity_y];
-    gyro.z = imu_sensor_state_[PantherImuSensor::angular_velocity_z];
+    gyro.x = imu_sensor_state_[PhidgetImuSensor::angular_velocity_x];
+    gyro.y = imu_sensor_state_[PhidgetImuSensor::angular_velocity_y];
+    gyro.z = imu_sensor_state_[PhidgetImuSensor::angular_velocity_z];
     return gyro;
   }
 };
 
-class TestPantherImuSensor : public testing::Test
+class TestPhidgetImuSensor : public testing::Test
 {
 public:
-  TestPantherImuSensor();
-  ~TestPantherImuSensor();
+  TestPhidgetImuSensor();
+  ~TestPhidgetImuSensor();
 
   void CreateResourceManagerFromUrdf(const std::string & urdf);
 
-  hardware_interface::return_type ConfigurePantherImu();
+  hardware_interface::return_type ConfigurePhidgetImu();
 
-  hardware_interface::return_type UnconfigurePantherImu();
+  hardware_interface::return_type UnconfigurePhidgetImu();
 
-  hardware_interface::return_type ActivatePantherImu();
+  hardware_interface::return_type ActivatePhidgetImu();
 
-  hardware_interface::return_type DeactivatePantherImu();
+  hardware_interface::return_type DeactivatePhidgetImu();
 
-  hardware_interface::return_type ShutdownPantherImu();
+  hardware_interface::return_type ShutdownPhidgetImu();
 
   /**
    * @brief Creates and returns URDF as a string
@@ -156,7 +156,7 @@ class TestPantherImuSensor : public testing::Test
     const std::unordered_map<std::string, std::string> & param_map,
     const std::list<std::string> & interfaces_list);
 
-  std::string GetDefaultPantherImuUrdf();
+  std::string GetDefaultPhidgetImuUrdf();
 
 protected:
   /**
@@ -179,7 +179,7 @@ class TestPantherImuSensor : public testing::Test
 
   std::list<hardware_interface::LoanedStateInterface> ClaimGoodStateInterfaces();
 
-  inline static const std::string kPantherImuName = "imu";
+  inline static const std::string kPhidgetImuName = "imu";
 
   inline static const std::string kUrdfHeader = R"(<?xml version="1.0" encoding="utf-8"?>
 <robot name="Panther">
@@ -206,61 +206,61 @@ class TestPantherImuSensor : public testing::Test
     {"world_frame", "enu"}};
 
   inline static const std::string kPluginName =
-    "<plugin>panther_hardware_interfaces/PantherImuSensor</plugin>";
+    "<plugin>husarion_ugv_hardware_interfaces/PhidgetImuSensor</plugin>";
 
-  std::unique_ptr<PantherImuSensorWrapper> imu_sensor_;
+  std::unique_ptr<PhidgetImuSensorWrapper> imu_sensor_;
   std::shared_ptr<hardware_interface::ResourceManager> rm_;
 };
 
-TestPantherImuSensor::TestPantherImuSensor()
+TestPhidgetImuSensor::TestPhidgetImuSensor()
 {
-  imu_sensor_ = std::make_unique<PantherImuSensorWrapper>();
+  imu_sensor_ = std::make_unique<PhidgetImuSensorWrapper>();
   rclcpp::init(0, nullptr);
 }
 
-TestPantherImuSensor::~TestPantherImuSensor() { rclcpp::shutdown(); }
+TestPhidgetImuSensor::~TestPhidgetImuSensor() { rclcpp::shutdown(); }
 
-void TestPantherImuSensor::CreateResourceManagerFromUrdf(const std::string & urdf)
+void TestPhidgetImuSensor::CreateResourceManagerFromUrdf(const std::string & urdf)
 {
   rm_ = std::make_shared<hardware_interface::ResourceManager>(urdf);
 }
 
-hardware_interface::return_type TestPantherImuSensor::ConfigurePantherImu()
+hardware_interface::return_type TestPhidgetImuSensor::ConfigurePhidgetImu()
 {
   return SetState(
     lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
     hardware_interface::lifecycle_state_names::INACTIVE);
 }
 
-hardware_interface::return_type TestPantherImuSensor::UnconfigurePantherImu()
+hardware_interface::return_type TestPhidgetImuSensor::UnconfigurePhidgetImu()
 {
   return SetState(
     lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
     hardware_interface::lifecycle_state_names::UNCONFIGURED);
 }
 
-hardware_interface::return_type TestPantherImuSensor::ActivatePantherImu()
+hardware_interface::return_type TestPhidgetImuSensor::ActivatePhidgetImu()
 {
   return SetState(
     lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
     hardware_interface::lifecycle_state_names::ACTIVE);
 }
 
-hardware_interface::return_type TestPantherImuSensor::DeactivatePantherImu()
+hardware_interface::return_type TestPhidgetImuSensor::DeactivatePhidgetImu()
 {
   return SetState(
     lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
     hardware_interface::lifecycle_state_names::INACTIVE);
 }
 
-hardware_interface::return_type TestPantherImuSensor::ShutdownPantherImu()
+hardware_interface::return_type TestPhidgetImuSensor::ShutdownPhidgetImu()
 {
   return SetState(
     lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
     hardware_interface::lifecycle_state_names::FINALIZED);
 }
 
-std::string TestPantherImuSensor::BuildUrdf(
+std::string TestPhidgetImuSensor::BuildUrdf(
   const std::unordered_map<std::string, std::string> & param_map,
   const std::list<std::string> & interfaces_list)
 {
@@ -286,18 +286,18 @@ std::string TestPantherImuSensor::BuildUrdf(
   return urdf.str();
 }
 
-std::string TestPantherImuSensor::GetDefaultPantherImuUrdf()
+std::string TestPhidgetImuSensor::GetDefaultPhidgetImuUrdf()
 {
   return BuildUrdf(kImuObligatoryParams, kImuInterfaces);
 }
-hardware_interface::return_type TestPantherImuSensor::SetState(
+hardware_interface::return_type TestPhidgetImuSensor::SetState(
   const std::uint8_t state_id, const std::string & state_name)
 {
   rclcpp_lifecycle::State state(state_id, state_name);
-  return rm_->set_component_state(kPantherImuName, state);
+  return rm_->set_component_state(kPhidgetImuName, state);
 }
 
-hardware_interface::HardwareInfo TestPantherImuSensor::CreateExampleInterfaces(
+hardware_interface::HardwareInfo TestPhidgetImuSensor::CreateExampleInterfaces(
   const hardware_interface::HardwareInfo & info)
 {
   hardware_interface::HardwareInfo new_info(info);
@@ -312,11 +312,11 @@ hardware_interface::HardwareInfo TestPantherImuSensor::CreateExampleInterfaces(
   return new_info;
 }
 
-hardware_interface::HardwareInfo TestPantherImuSensor::CreateCorrectInterfaces(
+hardware_interface::HardwareInfo TestPhidgetImuSensor::CreateCorrectInterfaces(
   const hardware_interface::HardwareInfo & info)
 {
   hardware_interface::HardwareInfo new_info(info);
-  for (const auto & interface_name : TestPantherImuSensor::kImuInterfaces) {
+  for (const auto & interface_name : TestPhidgetImuSensor::kImuInterfaces) {
     hardware_interface::InterfaceInfo interface_info;
     interface_info.name = interface_name;
     new_info.sensors.front().state_interfaces.push_back(interface_info);
@@ -324,7 +324,7 @@ hardware_interface::HardwareInfo TestPantherImuSensor::CreateCorrectInterfaces(
   return new_info;
 }
 
-hardware_interface::HardwareInfo TestPantherImuSensor::AddMadgwickParameters(
+hardware_interface::HardwareInfo TestPhidgetImuSensor::AddMadgwickParameters(
   const hardware_interface::HardwareInfo & info)
 {
   hardware_interface::HardwareInfo new_info(info);
@@ -340,16 +340,16 @@ hardware_interface::HardwareInfo TestPantherImuSensor::AddMadgwickParameters(
   return new_info;
 }
 
-std::list<hardware_interface::LoanedStateInterface> TestPantherImuSensor::ClaimGoodStateInterfaces()
+std::list<hardware_interface::LoanedStateInterface> TestPhidgetImuSensor::ClaimGoodStateInterfaces()
 {
   std::list<hardware_interface::LoanedStateInterface> list;
   for (const auto & interface_name : kImuInterfaces) {
-    list.push_back(rm_->claim_state_interface(kPantherImuName + "/" + interface_name));
+    list.push_back(rm_->claim_state_interface(kPhidgetImuName + "/" + interface_name));
   }
   return list;
 }
 
-TEST_F(TestPantherImuSensor, CheckSensorName)
+TEST_F(TestPhidgetImuSensor, CheckSensorName)
 {
   hardware_interface::HardwareInfo info;
   imu_sensor_->SetHardwareInfo(info);
@@ -363,7 +363,7 @@ TEST_F(TestPantherImuSensor, CheckSensorName)
   EXPECT_NO_THROW({ imu_sensor_->CheckSensorName(); });
 }
 
-TEST_F(TestPantherImuSensor, CheckStatesSize)
+TEST_F(TestPhidgetImuSensor, CheckStatesSize)
 {
   hardware_interface::HardwareInfo info;
   info.sensors.push_back({});
@@ -377,7 +377,7 @@ TEST_F(TestPantherImuSensor, CheckStatesSize)
   EXPECT_NO_THROW({ imu_sensor_->CheckStatesSize(); });
 }
 
-TEST_F(TestPantherImuSensor, CheckInterfaces)
+TEST_F(TestPhidgetImuSensor, CheckInterfaces)
 {
   hardware_interface::HardwareInfo info;
   info.sensors.push_back({});
@@ -395,25 +395,25 @@ TEST_F(TestPantherImuSensor, CheckInterfaces)
   EXPECT_NO_THROW({ imu_sensor_->CheckInterfaces(); });
 }
 
-TEST_F(TestPantherImuSensor, ReadObligatoryParams)
+TEST_F(TestPhidgetImuSensor, ReadObligatoryParams)
 {
   hardware_interface::HardwareInfo info;
   info.hardware_parameters = {{"param1", "value1"}, {"param2", "value2"}};
   imu_sensor_->SetHardwareInfo(info);
   EXPECT_THROW({ imu_sensor_->ReadObligatoryParams(); }, std::exception);
 
-  info.hardware_parameters = TestPantherImuSensor::kImuObligatoryParams;
+  info.hardware_parameters = TestPhidgetImuSensor::kImuObligatoryParams;
   info.hardware_parameters["callback_delta_epsilon_ms"] =
     info.hardware_parameters["data_interval_ms"];
   imu_sensor_->SetHardwareInfo(info);
   EXPECT_THROW({ imu_sensor_->ReadObligatoryParams(); }, std::exception);
 
-  info.hardware_parameters = TestPantherImuSensor::kImuObligatoryParams;
+  info.hardware_parameters = TestPhidgetImuSensor::kImuObligatoryParams;
   imu_sensor_->SetHardwareInfo(info);
   EXPECT_NO_THROW({ imu_sensor_->ReadObligatoryParams(); });
 }
 
-TEST_F(TestPantherImuSensor, CheckMagnitudeWrongValueAndCalibration)
+TEST_F(TestPhidgetImuSensor, CheckMagnitudeWrongValueAndCalibration)
 {
   using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
   hardware_interface::HardwareInfo info;
@@ -425,7 +425,7 @@ TEST_F(TestPantherImuSensor, CheckMagnitudeWrongValueAndCalibration)
   ASSERT_EQ(imu_sensor_->on_init(info), CallbackReturn::SUCCESS);
 
   double magnitude[3];
-  magnitude[0] = panther_hardware_interfaces::PantherImuSensor::KImuMagneticFieldUnknownValue;
+  magnitude[0] = husarion_ugv_hardware_interfaces::PhidgetImuSensor::KImuMagneticFieldUnknownValue;
 
   const auto wrong_magnitude_parsed = imu_sensor_->ParseMagnitude(magnitude);
   ASSERT_FALSE(imu_sensor_->IsVectorFinite(wrong_magnitude_parsed));
@@ -439,7 +439,7 @@ TEST_F(TestPantherImuSensor, CheckMagnitudeWrongValueAndCalibration)
   ASSERT_TRUE(imu_sensor_->IsIMUCalibrated(magnitude_parsed));
 }
 
-TEST_F(TestPantherImuSensor, CheckCalibrationOnDataCallbackAndAlgorithmInitialization)
+TEST_F(TestPhidgetImuSensor, CheckCalibrationOnDataCallbackAndAlgorithmInitialization)
 {
   using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
   hardware_interface::HardwareInfo info;
@@ -451,7 +451,7 @@ TEST_F(TestPantherImuSensor, CheckCalibrationOnDataCallbackAndAlgorithmInitializ
 
   ASSERT_EQ(imu_sensor_->on_init(info), CallbackReturn::SUCCESS);
   double magnitude[3], acceleration[3], gyration[3];
-  magnitude[0] = panther_hardware_interfaces::PantherImuSensor::KImuMagneticFieldUnknownValue;
+  magnitude[0] = husarion_ugv_hardware_interfaces::PhidgetImuSensor::KImuMagneticFieldUnknownValue;
   const auto fake_wrong_magnitude_parsed = imu_sensor_->ParseMagnitude(magnitude);
 
   ASSERT_FALSE(imu_sensor_->IsIMUCalibrated(fake_wrong_magnitude_parsed));
@@ -481,7 +481,7 @@ TEST_F(TestPantherImuSensor, CheckCalibrationOnDataCallbackAndAlgorithmInitializ
   ASSERT_TRUE(imu_sensor_->IsVectorFinite(imu_sensor_->GetGyration()));
 }
 
-TEST_F(TestPantherImuSensor, CheckReconnection)
+TEST_F(TestPhidgetImuSensor, CheckReconnection)
 {
   using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
   hardware_interface::HardwareInfo info;
@@ -493,7 +493,7 @@ TEST_F(TestPantherImuSensor, CheckReconnection)
 
   ASSERT_EQ(imu_sensor_->on_init(info), CallbackReturn::SUCCESS);
   double magnitude[3], acceleration[3], gyration[3];
-  magnitude[0] = panther_hardware_interfaces::PantherImuSensor::KImuMagneticFieldUnknownValue;
+  magnitude[0] = husarion_ugv_hardware_interfaces::PhidgetImuSensor::KImuMagneticFieldUnknownValue;
 
   // Correct values read from sensor
   magnitude[0] = -0.09675;
@@ -530,9 +530,9 @@ TEST_F(TestPantherImuSensor, CheckReconnection)
   ASSERT_TRUE(imu_sensor_->IsVectorFinite(imu_sensor_->GetGyration()));
 }
 
-TEST_F(TestPantherImuSensor, CheckInterfacesLoadedByResourceManager)
+TEST_F(TestPhidgetImuSensor, CheckInterfacesLoadedByResourceManager)
 {
-  CreateResourceManagerFromUrdf(GetDefaultPantherImuUrdf());
+  CreateResourceManagerFromUrdf(GetDefaultPhidgetImuUrdf());
 
   EXPECT_EQ(rm_->sensor_components_size(), 1u);
   ASSERT_EQ(rm_->state_interface_keys().size(), 10u);
@@ -551,14 +551,14 @@ TEST_F(TestPantherImuSensor, CheckInterfacesLoadedByResourceManager)
   EXPECT_TRUE(rm_->state_interface_exists("imu/linear_acceleration.z"));
 }
 
-TEST_F(TestPantherImuSensor, CheckStatesInitialValues)
+TEST_F(TestPhidgetImuSensor, CheckStatesInitialValues)
 {
   using hardware_interface::LoanedStateInterface;
   using hardware_interface::return_type;
 
-  CreateResourceManagerFromUrdf(GetDefaultPantherImuUrdf());
+  CreateResourceManagerFromUrdf(GetDefaultPhidgetImuUrdf());
 
-  ASSERT_EQ(ConfigurePantherImu(), return_type::OK);
+  ASSERT_EQ(ConfigurePhidgetImu(), return_type::OK);
 
   auto loaded_state_interfaces = ClaimGoodStateInterfaces();
 
@@ -566,29 +566,29 @@ TEST_F(TestPantherImuSensor, CheckStatesInitialValues)
     EXPECT_TRUE(std::isnan(state_interface.get_value()));
   }
 
-  EXPECT_EQ(ShutdownPantherImu(), return_type::OK);
+  EXPECT_EQ(ShutdownPhidgetImu(), return_type::OK);
 }
 
-TEST_F(TestPantherImuSensor, CheckWrongConfigurationWithWrongParameters)
+TEST_F(TestPhidgetImuSensor, CheckWrongConfigurationWithWrongParameters)
 {
   using hardware_interface::return_type;
 
-  const std::string panther_system_urdf_ = BuildUrdf({}, TestPantherImuSensor::kImuInterfaces);
-  CreateResourceManagerFromUrdf(panther_system_urdf_);
+  const std::string robot_system_urdf_ = BuildUrdf({}, TestPhidgetImuSensor::kImuInterfaces);
+  CreateResourceManagerFromUrdf(robot_system_urdf_);
 
-  EXPECT_EQ(ConfigurePantherImu(), return_type::ERROR);
-  EXPECT_EQ(ShutdownPantherImu(), return_type::OK);
+  EXPECT_EQ(ConfigurePhidgetImu(), return_type::ERROR);
+  EXPECT_EQ(ShutdownPhidgetImu(), return_type::OK);
 }
 
-TEST_F(TestPantherImuSensor, CheckReadAndConfigureRealSensor)
+TEST_F(TestPhidgetImuSensor, CheckReadAndConfigureRealSensor)
 {
   using hardware_interface::LoanedStateInterface;
   using hardware_interface::return_type;
 
-  CreateResourceManagerFromUrdf(GetDefaultPantherImuUrdf());
-  EXPECT_EQ(ConfigurePantherImu(), return_type::OK);
+  CreateResourceManagerFromUrdf(GetDefaultPhidgetImuUrdf());
+  EXPECT_EQ(ConfigurePhidgetImu(), return_type::OK);
 
-  ASSERT_EQ(ActivatePantherImu(), return_type::OK);
+  ASSERT_EQ(ActivatePhidgetImu(), return_type::OK);
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
   auto loaded_state_interfaces = ClaimGoodStateInterfaces();
@@ -596,8 +596,8 @@ TEST_F(TestPantherImuSensor, CheckReadAndConfigureRealSensor)
     EXPECT_TRUE(std::isfinite(state_interface.get_value()));
   }
 
-  EXPECT_EQ(UnconfigurePantherImu(), return_type::OK);
-  EXPECT_EQ(ShutdownPantherImu(), return_type::OK);
+  EXPECT_EQ(UnconfigurePhidgetImu(), return_type::OK);
+  EXPECT_EQ(ShutdownPhidgetImu(), return_type::OK);
 }
 
 int main(int argc, char ** argv)
diff --git a/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_controller.cpp b/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp
similarity index 93%
rename from panther_hardware_interfaces/test/panther_system/gpio/test_gpio_controller.cpp
rename to husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp
index aa50bf30..3ee2069d 100644
--- a/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_controller.cpp
+++ b/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp
@@ -20,12 +20,12 @@
 
 #include <gpiod.hpp>
 
-#include <panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp>
+#include <husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp>
 
-using GPIOInfo = panther_hardware_interfaces::GPIOInfo;
-using GPIOPin = panther_hardware_interfaces::GPIOPin;
+using GPIOInfo = husarion_ugv_hardware_interfaces::GPIOInfo;
+using GPIOPin = husarion_ugv_hardware_interfaces::GPIOPin;
 
-class MockGPIODriver : public panther_hardware_interfaces::GPIODriverInterface
+class MockGPIODriver : public husarion_ugv_hardware_interfaces::GPIODriverInterface
 {
 public:
   MOCK_METHOD(
@@ -42,11 +42,11 @@ class MockGPIODriver : public panther_hardware_interfaces::GPIODriverInterface
   MOCK_METHOD(bool, SetPinValue, (const GPIOPin pin, const bool value), (override));
 };
 
-class GPIOControllerWrapper : public panther_hardware_interfaces::GPIOControllerPTH12X
+class GPIOControllerWrapper : public husarion_ugv_hardware_interfaces::GPIOControllerPTH12X
 {
 public:
   GPIOControllerWrapper(std::shared_ptr<MockGPIODriver> gpio_driver)
-  : panther_hardware_interfaces::GPIOControllerPTH12X(gpio_driver)
+  : husarion_ugv_hardware_interfaces::GPIOControllerPTH12X(gpio_driver)
   {
   }
 
diff --git a/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_driver.cpp b/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_driver.cpp
similarity index 84%
rename from panther_hardware_interfaces/test/panther_system/gpio/test_gpio_driver.cpp
rename to husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_driver.cpp
index aede6036..2bff4cab 100644
--- a/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_driver.cpp
+++ b/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_driver.cpp
@@ -24,11 +24,11 @@
 #include <gtest/gtest.h>
 #include <gpiod.hpp>
 
-#include "panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp"
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
-using GPIOInfo = panther_hardware_interfaces::GPIOInfo;
-using GPIOPin = panther_hardware_interfaces::GPIOPin;
+using GPIOInfo = husarion_ugv_hardware_interfaces::GPIOInfo;
+using GPIOPin = husarion_ugv_hardware_interfaces::GPIOPin;
 
 class TestGPIODriver : public ::testing::Test
 {
@@ -43,7 +43,7 @@ class TestGPIODriver : public ::testing::Test
   void TearDown() override;
   void SetAndVerifyPinState(const GPIOPin & pin);
 
-  std::unique_ptr<panther_hardware_interfaces::GPIODriver> gpio_driver_;
+  std::unique_ptr<husarion_ugv_hardware_interfaces::GPIODriver> gpio_driver_;
   std::pair<GPIOPin, gpiod::line::value> last_gpio_event_summary_;
   const std::vector<GPIOInfo> gpio_config_info_{
     GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT},
@@ -52,7 +52,7 @@ class TestGPIODriver : public ::testing::Test
 
 void TestGPIODriver::SetUp()
 {
-  gpio_driver_ = std::make_unique<panther_hardware_interfaces::GPIODriver>(gpio_config_info_);
+  gpio_driver_ = std::make_unique<husarion_ugv_hardware_interfaces::GPIODriver>(gpio_config_info_);
 
   last_gpio_event_summary_.first = static_cast<GPIOPin>(-1);
   last_gpio_event_summary_.second = static_cast<gpiod::line::value>(-1);
@@ -80,7 +80,7 @@ TEST(TestGPIODriverInitialization, EmptyInfoStorage)
   EXPECT_THROW(
     {
       auto gpio_driver =
-        std::make_unique<panther_hardware_interfaces::GPIODriver>(std::vector<GPIOInfo>{});
+        std::make_unique<husarion_ugv_hardware_interfaces::GPIODriver>(std::vector<GPIOInfo>{});
     },
     std::runtime_error);
 }
@@ -90,11 +90,11 @@ TEST(TestGPIODriverInitialization, WrongPinConfigFail)
   // There is no OS version that supports simultaneous operation of MOTOR_ON and VMOT_ON pins.
   EXPECT_THROW(
     {
-      auto gpio_driver = std::make_unique<panther_hardware_interfaces::GPIODriver>(
+      auto gpio_driver = std::make_unique<husarion_ugv_hardware_interfaces::GPIODriver>(
         std::vector<GPIOInfo>{{GPIOPin::MOTOR_ON, gpiod::line::direction::OUTPUT}});
       gpio_driver.reset();
 
-      gpio_driver = std::make_unique<panther_hardware_interfaces::GPIODriver>(
+      gpio_driver = std::make_unique<husarion_ugv_hardware_interfaces::GPIODriver>(
         std::vector<GPIOInfo>{{GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT}});
     },
     std::invalid_argument);
@@ -102,7 +102,7 @@ TEST(TestGPIODriverInitialization, WrongPinConfigFail)
 
 TEST_F(TestGPIODriver, SetWrongPinValue)
 {
-  auto is_message_thrown = panther_utils::test_utils::IsMessageThrown<std::invalid_argument>(
+  auto is_message_thrown = husarion_ugv_utils::test_utils::IsMessageThrown<std::invalid_argument>(
     [&]() { this->gpio_driver_->SetPinValue(static_cast<GPIOPin>(-1), true); },
     "Pin not found in GPIO info storage.");
 
@@ -132,7 +132,7 @@ TEST_F(TestGPIODriver, IsPinActive)
 
 TEST_F(TestGPIODriver, IsPinActiveNoMonitorThread)
 {
-  auto is_message_thrown = panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  auto is_message_thrown = husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { this->gpio_driver_->IsPinActive(GPIOPin::LED_SBC_SEL); },
     "GPIO monitor thread is not running!");
 
@@ -166,7 +166,7 @@ TEST_F(TestGPIODriver, GPIOMonitorEnableUseRT)
 
 TEST_F(TestGPIODriver, GPIOEventCallbackNoMonitorThread)
 {
-  auto is_message_thrown = panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  auto is_message_thrown = husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() {
       this->gpio_driver_->ConfigureEdgeEventCallback(
         std::bind(&TestGPIODriver::GPIOEventCallback, this, std::placeholders::_1));
@@ -202,7 +202,7 @@ TEST_F(TestGPIODriver, ChangePinDirection)
   this->gpio_driver_->GPIOMonitorEnable();
   this->gpio_driver_->ChangePinDirection(GPIOPin::LED_SBC_SEL, gpiod::line::direction::INPUT);
 
-  auto is_message_thrown = panther_utils::test_utils::IsMessageThrown<std::invalid_argument>(
+  auto is_message_thrown = husarion_ugv_utils::test_utils::IsMessageThrown<std::invalid_argument>(
     [&]() { this->gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, true); },
     "Cannot set value for INPUT pin.");
 
diff --git a/panther_hardware_interfaces/test/test_utils.cpp b/husarion_ugv_hardware_interfaces/test/test_utils.cpp
similarity index 86%
rename from panther_hardware_interfaces/test/test_utils.cpp
rename to husarion_ugv_hardware_interfaces/test/test_utils.cpp
index 43f8c457..8d6f0f88 100644
--- a/panther_hardware_interfaces/test/test_utils.cpp
+++ b/husarion_ugv_hardware_interfaces/test/test_utils.cpp
@@ -19,11 +19,11 @@
 
 #include <gtest/gtest.h>
 
-#include <panther_hardware_interfaces/utils.hpp>
+#include <husarion_ugv_hardware_interfaces/utils.hpp>
 
 TEST(TestUtils, GetByte)
 {
-  using panther_hardware_interfaces::GetByte;
+  using husarion_ugv_hardware_interfaces::GetByte;
 
   EXPECT_EQ(GetByte(static_cast<std::int32_t>(0xFA3B4186), 0), 0x86);
   EXPECT_EQ(GetByte(static_cast<std::int32_t>(0xFA3B4186), 1), 0x41);
@@ -33,7 +33,7 @@ TEST(TestUtils, GetByte)
 
 TEST(TestUtils, GetByteOutOfRange)
 {
-  using panther_hardware_interfaces::GetByte;
+  using husarion_ugv_hardware_interfaces::GetByte;
 
   EXPECT_THROW(GetByte(static_cast<std::int32_t>(0xFA3B4186), 4), std::runtime_error);
   EXPECT_THROW(GetByte(static_cast<std::int32_t>(0xFA3B4186), -1), std::runtime_error);
@@ -45,7 +45,7 @@ TEST(TestUtils, OperationWithAttemptsFailTest)
   unsigned attempts_counter = 0;
   unsigned on_error_counter = 0;
 
-  EXPECT_FALSE(panther_hardware_interfaces::OperationWithAttempts(
+  EXPECT_FALSE(husarion_ugv_hardware_interfaces::OperationWithAttempts(
     [&attempts_counter]() {
       ++attempts_counter;
       throw std::runtime_error("");
@@ -60,7 +60,7 @@ TEST(TestUtils, OperationWithAttemptsSuccessTest)
   unsigned max_attempts = 5;
   unsigned attempts_counter = 0;
 
-  EXPECT_TRUE(panther_hardware_interfaces::OperationWithAttempts(
+  EXPECT_TRUE(husarion_ugv_hardware_interfaces::OperationWithAttempts(
     [&attempts_counter, &max_attempts]() {
       ++attempts_counter;
       if (attempts_counter < max_attempts) {
@@ -73,13 +73,13 @@ TEST(TestUtils, OperationWithAttemptsSuccessTest)
 
 TEST(TestUtils, OperationWithAttemptsOnErrorThrowTest)
 {
-  EXPECT_FALSE(panther_hardware_interfaces::OperationWithAttempts(
+  EXPECT_FALSE(husarion_ugv_hardware_interfaces::OperationWithAttempts(
     []() { throw std::runtime_error(""); }, 5, []() { throw std::runtime_error(""); }));
 }
 
 TEST(TestUtils, CheckIfJointNameContainValidSequence)
 {
-  using panther_hardware_interfaces::CheckIfJointNameContainValidSequence;
+  using husarion_ugv_hardware_interfaces::CheckIfJointNameContainValidSequence;
 
   EXPECT_TRUE(CheckIfJointNameContainValidSequence("fr_wheel_joint", "fr"));
   EXPECT_TRUE(CheckIfJointNameContainValidSequence("namespace/fr_wheel_joint", "fr"));
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_canopen_manager.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_canopen_manager.cpp
similarity index 74%
rename from panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_canopen_manager.cpp
rename to husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_canopen_manager.cpp
index 51d5ec2b..6db65c90 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_canopen_manager.cpp
+++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_canopen_manager.cpp
@@ -19,12 +19,12 @@
 #include <gtest/gtest.h>
 #include <sys/socket.h>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp"
 
 #include "utils/fake_can_socket.hpp"
 #include "utils/test_constants.hpp"
 
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
 class TestCANopenManager : public ::testing::Test
 {
@@ -34,17 +34,17 @@ class TestCANopenManager : public ::testing::Test
   ~TestCANopenManager() {}
 
 protected:
-  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
-  std::unique_ptr<panther_hardware_interfaces::CANopenManager> canopen_manager_;
+  std::unique_ptr<husarion_ugv_hardware_interfaces_test::FakeCANSocket> can_socket_;
+  std::unique_ptr<husarion_ugv_hardware_interfaces::CANopenManager> canopen_manager_;
 };
 
 TestCANopenManager::TestCANopenManager()
 {
-  can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
-    panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
+  can_socket_ = std::make_unique<husarion_ugv_hardware_interfaces_test::FakeCANSocket>(
+    husarion_ugv_hardware_interfaces_test::kCANopenSettings.can_interface_name);
 
-  canopen_manager_ = std::make_unique<panther_hardware_interfaces::CANopenManager>(
-    panther_hardware_interfaces_test::kCANopenSettings);
+  canopen_manager_ = std::make_unique<husarion_ugv_hardware_interfaces::CANopenManager>(
+    husarion_ugv_hardware_interfaces_test::kCANopenSettings);
 }
 
 TEST_F(TestCANopenManager, InitializeAndDeinitialize)
@@ -71,7 +71,7 @@ TEST_F(TestCANopenManager, Activate)
 
 TEST_F(TestCANopenManager, ActivateNotInitialized)
 {
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { canopen_manager_->Activate(); }, "CANopenManager not initialized."));
 }
 
@@ -90,7 +90,7 @@ TEST_F(TestCANopenManager, GetMaster)
 
 TEST_F(TestCANopenManager, GetMasterNotInitialized)
 {
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { canopen_manager_->GetMaster(); }, "CANopenManager not initialized."));
 }
 
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_lynx_robot_driver.cpp
similarity index 58%
rename from panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
rename to husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_lynx_robot_driver.cpp
index da5706cd..59918387 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
+++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_lynx_robot_driver.cpp
@@ -20,48 +20,49 @@
 
 #include <gmock/gmock.h>
 
-#include <panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp>
-#include <panther_hardware_interfaces/panther_system/robot_driver/driver.hpp>
-#include <panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp>
-#include <panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp>
+#include <husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp>
+#include <husarion_ugv_hardware_interfaces/robot_system/robot_driver/driver.hpp>
+#include <husarion_ugv_hardware_interfaces/robot_system/robot_driver/lynx_robot_driver.hpp>
+#include <husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp>
 
 #include "utils/fake_can_socket.hpp"
 #include "utils/mock_driver.hpp"
 #include "utils/test_constants.hpp"
 
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
-class LynxRobotDriverWrapper : public panther_hardware_interfaces::LynxRobotDriver
+class LynxRobotDriverWrapper : public husarion_ugv_hardware_interfaces::LynxRobotDriver
 {
 public:
   LynxRobotDriverWrapper(
-    const panther_hardware_interfaces::CANopenSettings & canopen_settings,
-    const panther_hardware_interfaces::DrivetrainSettings & drivetrain_settings,
+    const husarion_ugv_hardware_interfaces::CANopenSettings & canopen_settings,
+    const husarion_ugv_hardware_interfaces::DrivetrainSettings & drivetrain_settings,
     const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000))
   : LynxRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time)
   {
-    mock_left_motor_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
-    mock_right_motor_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+    mock_left_motor_driver = std::make_shared<
+      ::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockMotorDriver>>();
+    mock_right_motor_driver = std::make_shared<
+      ::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockMotorDriver>>();
 
     mock_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
+      std::make_shared<::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockDriver>>();
     mock_driver->AddMotorDriver(
-      panther_hardware_interfaces::MotorNames::LEFT, mock_left_motor_driver);
+      husarion_ugv_hardware_interfaces::MotorNames::LEFT, mock_left_motor_driver);
     mock_driver->AddMotorDriver(
-      panther_hardware_interfaces::MotorNames::RIGHT, mock_right_motor_driver);
+      husarion_ugv_hardware_interfaces::MotorNames::RIGHT, mock_right_motor_driver);
   }
 
   void DefineDrivers() override
   {
-    drivers_.emplace(panther_hardware_interfaces::DriverNames::DEFAULT, mock_driver);
+    drivers_.emplace(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT, mock_driver);
   }
 
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>> mock_driver;
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  std::shared_ptr<::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockDriver>>
+    mock_driver;
+  std::shared_ptr<::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockMotorDriver>>
     mock_left_motor_driver;
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  std::shared_ptr<::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockMotorDriver>>
     mock_right_motor_driver;
 };
 
@@ -70,13 +71,13 @@ class TestLynxRobotDriver : public ::testing::Test
 public:
   TestLynxRobotDriver()
   {
-    can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
-      panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
+    can_socket_ = std::make_unique<husarion_ugv_hardware_interfaces_test::FakeCANSocket>(
+      husarion_ugv_hardware_interfaces_test::kCANopenSettings.can_interface_name);
     can_socket_->Initialize();
 
     robot_driver_ = std::make_unique<LynxRobotDriverWrapper>(
-      panther_hardware_interfaces_test::kCANopenSettings,
-      panther_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10));
+      husarion_ugv_hardware_interfaces_test::kCANopenSettings,
+      husarion_ugv_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10));
 
     robot_driver_->Initialize();
     robot_driver_->Activate();
@@ -85,13 +86,13 @@ class TestLynxRobotDriver : public ::testing::Test
   ~TestLynxRobotDriver() { robot_driver_->Deinitialize(); }
 
 protected:
-  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
+  std::unique_ptr<husarion_ugv_hardware_interfaces_test::FakeCANSocket> can_socket_;
   std::unique_ptr<LynxRobotDriverWrapper> robot_driver_;
 };
 
 TEST_F(TestLynxRobotDriver, SendSpeedCommands)
 {
-  using panther_hardware_interfaces_test::kRadPerSecToRbtqCmd;
+  using husarion_ugv_hardware_interfaces_test::kRadPerSecToRbtqCmd;
 
   const float left_v = 0.1;
   const float right_v = 0.2;
@@ -115,7 +116,7 @@ TEST_F(TestLynxRobotDriver, SendSpeedCommandsSendCmdVelError)
   EXPECT_CALL(*robot_driver_->mock_left_motor_driver, SendCmdVel(::testing::_))
     .WillOnce(::testing::Throw(std::runtime_error("")));
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { robot_driver_->SendSpeedCommands({0.0, 0.0}); }, "Driver send Roboteq cmd failed:"));
 }
 
@@ -123,7 +124,7 @@ TEST_F(TestLynxRobotDriver, SendSpeedCommandsCANError)
 {
   EXPECT_CALL(*robot_driver_->mock_driver, IsCANError()).WillOnce(::testing::Return(true));
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { robot_driver_->SendSpeedCommands({0.0, 0.0}); },
     "CAN error detected on the Driver when trying to write speed commands."));
 }
@@ -132,7 +133,7 @@ TEST_F(TestLynxRobotDriver, SendSpeedCommandsInvalidVectorSize)
 {
   const std::vector<float> speeds = {0.1, 0.2, 0.3};
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { robot_driver_->SendSpeedCommands(speeds); }, "Invalid speeds vector size"));
 }
 
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_panther_robot_driver.cpp
similarity index 55%
rename from panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
rename to husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_panther_robot_driver.cpp
index 7ba08f5f..4663958e 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
+++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_panther_robot_driver.cpp
@@ -20,67 +20,67 @@
 
 #include <gmock/gmock.h>
 
-#include <panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp>
-#include <panther_hardware_interfaces/panther_system/robot_driver/driver.hpp>
-#include <panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp>
-#include <panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp>
+#include <husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp>
+#include <husarion_ugv_hardware_interfaces/robot_system/robot_driver/driver.hpp>
+#include <husarion_ugv_hardware_interfaces/robot_system/robot_driver/panther_robot_driver.hpp>
+#include <husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp>
 
 #include "utils/fake_can_socket.hpp"
 #include "utils/mock_driver.hpp"
 #include "utils/test_constants.hpp"
 
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
-class PantherRobotDriverWrapper : public panther_hardware_interfaces::PantherRobotDriver
+class PantherRobotDriverWrapper : public husarion_ugv_hardware_interfaces::PantherRobotDriver
 {
 public:
   PantherRobotDriverWrapper(
-    const panther_hardware_interfaces::CANopenSettings & canopen_settings,
-    const panther_hardware_interfaces::DrivetrainSettings & drivetrain_settings,
+    const husarion_ugv_hardware_interfaces::CANopenSettings & canopen_settings,
+    const husarion_ugv_hardware_interfaces::DrivetrainSettings & drivetrain_settings,
     const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000))
   : PantherRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time)
   {
-    mock_fl_motor_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
-    mock_fr_motor_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
-    mock_rl_motor_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
-    mock_rr_motor_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>();
+    mock_fl_motor_driver = std::make_shared<
+      ::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockMotorDriver>>();
+    mock_fr_motor_driver = std::make_shared<
+      ::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockMotorDriver>>();
+    mock_rl_motor_driver = std::make_shared<
+      ::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockMotorDriver>>();
+    mock_rr_motor_driver = std::make_shared<
+      ::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockMotorDriver>>();
 
     mock_front_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
+      std::make_shared<::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockDriver>>();
     mock_front_driver->AddMotorDriver(
-      panther_hardware_interfaces::MotorNames::LEFT, mock_fl_motor_driver);
+      husarion_ugv_hardware_interfaces::MotorNames::LEFT, mock_fl_motor_driver);
     mock_front_driver->AddMotorDriver(
-      panther_hardware_interfaces::MotorNames::RIGHT, mock_fr_motor_driver);
+      husarion_ugv_hardware_interfaces::MotorNames::RIGHT, mock_fr_motor_driver);
 
     mock_rear_driver =
-      std::make_shared<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>();
+      std::make_shared<::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockDriver>>();
     mock_rear_driver->AddMotorDriver(
-      panther_hardware_interfaces::MotorNames::LEFT, mock_rl_motor_driver);
+      husarion_ugv_hardware_interfaces::MotorNames::LEFT, mock_rl_motor_driver);
     mock_rear_driver->AddMotorDriver(
-      panther_hardware_interfaces::MotorNames::RIGHT, mock_rr_motor_driver);
+      husarion_ugv_hardware_interfaces::MotorNames::RIGHT, mock_rr_motor_driver);
   }
 
   void DefineDrivers() override
   {
-    drivers_.emplace(panther_hardware_interfaces::DriverNames::FRONT, mock_front_driver);
-    drivers_.emplace(panther_hardware_interfaces::DriverNames::REAR, mock_rear_driver);
+    drivers_.emplace(husarion_ugv_hardware_interfaces::DriverNames::FRONT, mock_front_driver);
+    drivers_.emplace(husarion_ugv_hardware_interfaces::DriverNames::REAR, mock_rear_driver);
   }
 
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
+  std::shared_ptr<::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockDriver>>
     mock_front_driver;
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockDriver>>
+  std::shared_ptr<::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockDriver>>
     mock_rear_driver;
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  std::shared_ptr<::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockMotorDriver>>
     mock_fl_motor_driver;
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  std::shared_ptr<::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockMotorDriver>>
     mock_fr_motor_driver;
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  std::shared_ptr<::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockMotorDriver>>
     mock_rl_motor_driver;
-  std::shared_ptr<::testing::NiceMock<panther_hardware_interfaces_test::MockMotorDriver>>
+  std::shared_ptr<::testing::NiceMock<husarion_ugv_hardware_interfaces_test::MockMotorDriver>>
     mock_rr_motor_driver;
 };
 
@@ -89,13 +89,13 @@ class TestPantherRobotDriver : public ::testing::Test
 public:
   TestPantherRobotDriver()
   {
-    can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
-      panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
+    can_socket_ = std::make_unique<husarion_ugv_hardware_interfaces_test::FakeCANSocket>(
+      husarion_ugv_hardware_interfaces_test::kCANopenSettings.can_interface_name);
     can_socket_->Initialize();
 
     robot_driver_ = std::make_unique<PantherRobotDriverWrapper>(
-      panther_hardware_interfaces_test::kCANopenSettings,
-      panther_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10));
+      husarion_ugv_hardware_interfaces_test::kCANopenSettings,
+      husarion_ugv_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10));
 
     robot_driver_->Initialize();
     robot_driver_->Activate();
@@ -104,13 +104,13 @@ class TestPantherRobotDriver : public ::testing::Test
   ~TestPantherRobotDriver() { robot_driver_->Deinitialize(); }
 
 protected:
-  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
+  std::unique_ptr<husarion_ugv_hardware_interfaces_test::FakeCANSocket> can_socket_;
   std::unique_ptr<PantherRobotDriverWrapper> robot_driver_;
 };
 
 TEST_F(TestPantherRobotDriver, SendSpeedCommands)
 {
-  using panther_hardware_interfaces_test::kRadPerSecToRbtqCmd;
+  using husarion_ugv_hardware_interfaces_test::kRadPerSecToRbtqCmd;
 
   const float fl_v = 0.1;
   const float fr_v = 0.2;
@@ -145,7 +145,7 @@ TEST_F(TestPantherRobotDriver, SendSpeedCommandsSendCmdVelError)
   EXPECT_CALL(*robot_driver_->mock_fl_motor_driver, SendCmdVel(::testing::_))
     .WillOnce(::testing::Throw(std::runtime_error("")));
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { robot_driver_->SendSpeedCommands({0.0, 0.0, 0.0, 0.0}); },
     "Front driver send Roboteq cmd failed:"));
 }
@@ -154,7 +154,7 @@ TEST_F(TestPantherRobotDriver, SendSpeedCommandsCANError)
 {
   EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).WillOnce(::testing::Return(true));
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { robot_driver_->SendSpeedCommands({0.0, 0.0, 0.0, 0.0}); },
     "CAN error detected on the front driver when trying to write speed commands."));
 }
@@ -163,7 +163,7 @@ TEST_F(TestPantherRobotDriver, SendSpeedCommandsInvalidVectorSize)
 {
   const std::vector<float> speeds = {0.1, 0.2, 0.3};
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { robot_driver_->SendSpeedCommands(speeds); }, "Invalid speeds vector size"));
 }
 
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_data_converters.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_data_converters.cpp
similarity index 86%
rename from panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_data_converters.cpp
rename to husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_data_converters.cpp
index 1bba2723..6eecd75e 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_data_converters.cpp
+++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_data_converters.cpp
@@ -19,7 +19,7 @@
 
 #include <gtest/gtest.h>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp"
 
 #include "utils/test_constants.hpp"
 
@@ -70,8 +70,8 @@ void TestRuntimeErrorMsg(
 
 TEST(TestRoboteqDataConverters, CommandConverter)
 {
-  panther_hardware_interfaces::RoboteqVelocityCommandConverter cmd_converter(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
+  husarion_ugv_hardware_interfaces::RoboteqVelocityCommandConverter cmd_converter(
+    husarion_ugv_hardware_interfaces_test::kDrivetrainSettings);
 
   const float conversion_factor = 79.789678;
 
@@ -84,18 +84,18 @@ TEST(TestRoboteqDataConverters, CommandConverter)
 
 TEST(TestRoboteqDataConverters, MotorState)
 {
-  using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
-  using panther_hardware_interfaces_test::kRbtqPosFbToRad;
-  using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+  using husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  using husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad;
+  using husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
 
-  panther_hardware_interfaces::MotorState motor_state(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
+  husarion_ugv_hardware_interfaces::MotorState motor_state(
+    husarion_ugv_hardware_interfaces_test::kDrivetrainSettings);
 
   EXPECT_FLOAT_EQ(motor_state.GetPosition(), 0.0);
   EXPECT_FLOAT_EQ(motor_state.GetVelocity(), 0.0);
   EXPECT_FLOAT_EQ(motor_state.GetTorque(), 0.0);
 
-  panther_hardware_interfaces::MotorDriverState feedback1;
+  husarion_ugv_hardware_interfaces::MotorDriverState feedback1;
   feedback1.pos = 48128;
   feedback1.vel = 1000;
   feedback1.current = 1;
@@ -105,7 +105,7 @@ TEST(TestRoboteqDataConverters, MotorState)
   EXPECT_FLOAT_EQ(motor_state.GetVelocity(), feedback1.vel * kRbtqVelFbToRadPerSec);
   EXPECT_FLOAT_EQ(motor_state.GetTorque(), feedback1.current * kRbtqCurrentFbToNewtonMeters);
 
-  panther_hardware_interfaces::MotorDriverState feedback2;
+  husarion_ugv_hardware_interfaces::MotorDriverState feedback2;
   feedback2.pos = -48128;
   feedback2.vel = -1000;
   feedback2.current = -1;
@@ -118,7 +118,7 @@ TEST(TestRoboteqDataConverters, MotorState)
 
 TEST(TestRoboteqDataConverters, FlagError)
 {
-  panther_hardware_interfaces::FlagError flag_error(
+  husarion_ugv_hardware_interfaces::FlagError flag_error(
     {"error1", "error2", "error3", "error4", "error5", "error6", "error7", "error8"},
     {"error2", "error6"});
 
@@ -140,7 +140,7 @@ TEST(TestRoboteqDataConverters, FlagError)
 
 TEST(TestRoboteqDataConverters, FaultFlag)
 {
-  panther_hardware_interfaces::FaultFlag fault_flag;
+  husarion_ugv_hardware_interfaces::FaultFlag fault_flag;
 
   fault_flag.SetData(0b00000001);
   TestFaultFlagMsg(
@@ -173,7 +173,7 @@ TEST(TestRoboteqDataConverters, FaultFlag)
 
 TEST(TestRoboteqDataConverters, ScriptFlag)
 {
-  panther_hardware_interfaces::ScriptFlag script_flag;
+  husarion_ugv_hardware_interfaces::ScriptFlag script_flag;
 
   script_flag.SetData(0b00000001);
   TestScriptFlagMsg(script_flag.GetMessage(), {true, false, false});
@@ -188,7 +188,7 @@ TEST(TestRoboteqDataConverters, ScriptFlag)
 
 TEST(TestRoboteqDataConverters, RuntimeError)
 {
-  panther_hardware_interfaces::RuntimeError runtime_error;
+  husarion_ugv_hardware_interfaces::RuntimeError runtime_error;
 
   runtime_error.SetData(0b00000001);
   TestRuntimeErrorMsg(runtime_error.GetMessage(), {true, false, false, false, false, false, false});
@@ -215,7 +215,7 @@ TEST(TestRoboteqDataConverters, RuntimeError)
 
 TEST(TestRoboteqDataConverters, DriverState)
 {
-  panther_hardware_interfaces::RoboteqDriverState driver_state;
+  husarion_ugv_hardware_interfaces::RoboteqDriverState driver_state;
 
   EXPECT_FLOAT_EQ(driver_state.GetTemperature(), 0.0);
   EXPECT_FLOAT_EQ(driver_state.GetVoltage(), 0.0);
@@ -233,18 +233,19 @@ TEST(TestRoboteqDataConverters, DriverState)
 
 TEST(TestRoboteqDataConverters, DriverData)
 {
-  using panther_hardware_interfaces::RoboteqDriver;
-  using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
-  using panther_hardware_interfaces_test::kRbtqPosFbToRad;
-  using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+  using husarion_ugv_hardware_interfaces::RoboteqDriver;
+  using husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  using husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad;
+  using husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
 
-  panther_hardware_interfaces::DriverData roboteq_data(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
+  husarion_ugv_hardware_interfaces::DriverData roboteq_data(
+    husarion_ugv_hardware_interfaces_test::kDrivetrainSettings);
 
   ASSERT_FALSE(roboteq_data.IsError());
 
-  const panther_hardware_interfaces::MotorDriverState channel_1_state = {0, 0, 0, {0, 0}, {0, 0}};
-  const panther_hardware_interfaces::MotorDriverState channel_2_state = {
+  const husarion_ugv_hardware_interfaces::MotorDriverState channel_1_state = {
+    0, 0, 0, {0, 0}, {0, 0}};
+  const husarion_ugv_hardware_interfaces::MotorDriverState channel_2_state = {
     48128, 1000, 1, {0, 0}, {0, 0}};
 
   roboteq_data.SetMotorsStates(channel_1_state, channel_2_state, true);
@@ -277,7 +278,7 @@ TEST(TestRoboteqDataConverters, DriverData)
     roboteq_data.GetMotorState(RoboteqDriver::kChannel2).GetTorque(),
     channel_2_state.current * kRbtqCurrentFbToNewtonMeters);
 
-  panther_hardware_interfaces::DriverState state;
+  husarion_ugv_hardware_interfaces::DriverState state;
 
   state.fault_flags = 0b00000001;
   state.script_flags = 0b00000010;
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_driver.cpp
similarity index 80%
rename from panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
rename to husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_driver.cpp
index 288bc3d0..9468ab79 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_driver.cpp
+++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_driver.cpp
@@ -20,14 +20,14 @@
 
 #include <gtest/gtest.h>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp"
 
 #include "utils/fake_can_socket.hpp"
 #include "utils/mock_roboteq.hpp"
 #include "utils/test_constants.hpp"
 
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
 class TestRoboteqDriver : public ::testing::Test
 {
@@ -44,26 +44,26 @@ class TestRoboteqDriver : public ::testing::Test
   static constexpr char kMotor1Name[] = "motor_1";
   static constexpr char kMotor2Name[] = "motor_2";
 
-  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
-  std::unique_ptr<panther_hardware_interfaces_test::MockRoboteq> mock_roboteq_;
-  std::unique_ptr<panther_hardware_interfaces::CANopenManager> canopen_manager_;
-  std::shared_ptr<panther_hardware_interfaces::RoboteqDriver> roboteq_driver_;
+  std::unique_ptr<husarion_ugv_hardware_interfaces_test::FakeCANSocket> can_socket_;
+  std::unique_ptr<husarion_ugv_hardware_interfaces_test::MockRoboteq> mock_roboteq_;
+  std::unique_ptr<husarion_ugv_hardware_interfaces::CANopenManager> canopen_manager_;
+  std::shared_ptr<husarion_ugv_hardware_interfaces::RoboteqDriver> roboteq_driver_;
 };
 
 TestRoboteqDriver::TestRoboteqDriver()
 {
-  can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
-    panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
+  can_socket_ = std::make_unique<husarion_ugv_hardware_interfaces_test::FakeCANSocket>(
+    husarion_ugv_hardware_interfaces_test::kCANopenSettings.can_interface_name);
   can_socket_->Initialize();
 
-  canopen_manager_ = std::make_unique<panther_hardware_interfaces::CANopenManager>(
-    panther_hardware_interfaces_test::kCANopenSettings);
+  canopen_manager_ = std::make_unique<husarion_ugv_hardware_interfaces::CANopenManager>(
+    husarion_ugv_hardware_interfaces_test::kCANopenSettings);
 
-  mock_roboteq_ = std::make_unique<panther_hardware_interfaces_test::MockRoboteq>();
+  mock_roboteq_ = std::make_unique<husarion_ugv_hardware_interfaces_test::MockRoboteq>();
   mock_roboteq_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
   canopen_manager_->Initialize();
 
-  roboteq_driver_ = std::make_shared<panther_hardware_interfaces::RoboteqDriver>(
+  roboteq_driver_ = std::make_shared<husarion_ugv_hardware_interfaces::RoboteqDriver>(
     canopen_manager_->GetMaster(), 1, std::chrono::milliseconds(100));
 
   canopen_manager_->Activate();
@@ -80,10 +80,10 @@ TestRoboteqDriver::~TestRoboteqDriver()
 
 void TestRoboteqDriver::BootRoboteqDriver()
 {
-  auto motor_1 = std::make_shared<panther_hardware_interfaces::RoboteqMotorDriver>(
-    roboteq_driver_, panther_hardware_interfaces::RoboteqDriver::kChannel1);
-  auto motor_2 = std::make_shared<panther_hardware_interfaces::RoboteqMotorDriver>(
-    roboteq_driver_, panther_hardware_interfaces::RoboteqDriver::kChannel2);
+  auto motor_1 = std::make_shared<husarion_ugv_hardware_interfaces::RoboteqMotorDriver>(
+    roboteq_driver_, husarion_ugv_hardware_interfaces::RoboteqDriver::kChannel1);
+  auto motor_2 = std::make_shared<husarion_ugv_hardware_interfaces::RoboteqMotorDriver>(
+    roboteq_driver_, husarion_ugv_hardware_interfaces::RoboteqDriver::kChannel2);
 
   roboteq_driver_->AddMotorDriver(kMotor1Name, motor_1);
   roboteq_driver_->AddMotorDriver(kMotor2Name, motor_2);
@@ -130,7 +130,7 @@ TEST_F(TestRoboteqDriver, BootErrorVendorId)
 
 TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
 {
-  using panther_hardware_interfaces_test::DriverChannel;
+  using husarion_ugv_hardware_interfaces_test::DriverChannel;
 
   const std::int32_t motor_1_pos = 101;
   const std::int32_t motor_1_vel = 102;
@@ -152,9 +152,9 @@ TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates)
 
   std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
-  panther_hardware_interfaces::MotorDriverState motor_driver_state_1 =
+  husarion_ugv_hardware_interfaces::MotorDriverState motor_driver_state_1 =
     roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadState();
-  panther_hardware_interfaces::MotorDriverState motor_driver_state_2 =
+  husarion_ugv_hardware_interfaces::MotorDriverState motor_driver_state_2 =
     roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadState();
 
   EXPECT_EQ(motor_driver_state_1.pos, motor_1_pos);
@@ -170,13 +170,13 @@ TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps)
 {
   BootRoboteqDriver();
 
-  panther_hardware_interfaces::MotorDriverState motor_driver_state_1 =
+  husarion_ugv_hardware_interfaces::MotorDriverState motor_driver_state_1 =
     roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadState();
 
   // based on publishing frequency in the Roboteq mock (100Hz)
   std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
-  panther_hardware_interfaces::MotorDriverState motor_driver_state_2 =
+  husarion_ugv_hardware_interfaces::MotorDriverState motor_driver_state_2 =
     roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadState();
 
   // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
@@ -204,10 +204,10 @@ TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps)
 
 TEST_F(TestRoboteqDriver, ReadState)
 {
-  using panther_hardware_interfaces_test::DriverChannel;
-  using panther_hardware_interfaces_test::DriverFaultFlags;
-  using panther_hardware_interfaces_test::DriverRuntimeErrors;
-  using panther_hardware_interfaces_test::DriverScriptFlags;
+  using husarion_ugv_hardware_interfaces_test::DriverChannel;
+  using husarion_ugv_hardware_interfaces_test::DriverFaultFlags;
+  using husarion_ugv_hardware_interfaces_test::DriverRuntimeErrors;
+  using husarion_ugv_hardware_interfaces_test::DriverScriptFlags;
 
   const std::int16_t temp = 30;
   const std::int16_t heatsink_temp = 31;
@@ -230,7 +230,7 @@ TEST_F(TestRoboteqDriver, ReadState)
 
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  panther_hardware_interfaces::DriverState driver_state = roboteq_driver_->ReadState();
+  husarion_ugv_hardware_interfaces::DriverState driver_state = roboteq_driver_->ReadState();
 
   EXPECT_EQ(driver_state.mcu_temp, temp);
   EXPECT_EQ(driver_state.heatsink_temp, heatsink_temp);
@@ -248,12 +248,12 @@ TEST_F(TestRoboteqDriver, ReadStateTimestamp)
 {
   BootRoboteqDriver();
 
-  panther_hardware_interfaces::DriverState driver_state_1 = roboteq_driver_->ReadState();
+  husarion_ugv_hardware_interfaces::DriverState driver_state_1 = roboteq_driver_->ReadState();
 
   // based on publishing frequency in the Roboteq mock (20Hz)
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  panther_hardware_interfaces::DriverState driver_state_2 = roboteq_driver_->ReadState();
+  husarion_ugv_hardware_interfaces::DriverState driver_state_2 = roboteq_driver_->ReadState();
 
   // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked
   // if consecutive messages will have timestamps 100ms + some threshold apart
@@ -280,7 +280,7 @@ TEST_F(TestRoboteqDriver, ReadStateTimestamp)
 
 TEST_F(TestRoboteqDriver, SendRoboteqCmd)
 {
-  using panther_hardware_interfaces_test::DriverChannel;
+  using husarion_ugv_hardware_interfaces_test::DriverChannel;
 
   const std::int32_t motor_1_v = 10;
   const std::int32_t motor_2_v = 20;
@@ -310,7 +310,7 @@ TEST_F(TestRoboteqDriver, ResetRoboteqScriptSDOTimeoutReset)
   BootRoboteqDriver();
 
   mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 100000);
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { roboteq_driver_->ResetScript(); }, "SDO protocol timed out"));
 
   mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x2018, 0, 0);
@@ -330,7 +330,7 @@ TEST_F(TestRoboteqDriver, TurnOnEStopTimeout)
 {
   BootRoboteqDriver();
   mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200C, 0, 100000);
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { roboteq_driver_->TurnOnEStop(); }, "SDO protocol timed out"));
 }
 
@@ -347,7 +347,7 @@ TEST_F(TestRoboteqDriver, TurnOffEStopTimeout)
 {
   BootRoboteqDriver();
   mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x200D, 0, 100000);
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { roboteq_driver_->TurnOffEStop(); }, "SDO protocol timed out"));
 }
 
@@ -373,7 +373,7 @@ TEST_F(TestRoboteqDriver, WriteTimeout)
 {
   BootRoboteqDriver();
   mock_roboteq_->GetDriver()->SetOnWriteWait<std::uint8_t>(0x202C, 0, 200000);
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop(); },
     "SDO protocol timed out"));
 }
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_error_filter.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_error_filter.cpp
similarity index 87%
rename from panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_error_filter.cpp
rename to husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_error_filter.cpp
index cd3d6566..b4fe4ede 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_error_filter.cpp
+++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_error_filter.cpp
@@ -14,13 +14,13 @@
 
 #include <gtest/gtest.h>
 
-#include <panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp>
+#include <husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_error_filter.hpp>
 
 TEST(TestRoboteqErrorFilter, InitialState)
 {
-  using panther_hardware_interfaces::ErrorsFilterIds;
+  using husarion_ugv_hardware_interfaces::ErrorsFilterIds;
 
-  panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
+  husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
 
   ASSERT_FALSE(roboteq_error_filter.IsError());
   EXPECT_FALSE(roboteq_error_filter.IsError(ErrorsFilterIds::WRITE_PDO_CMDS));
@@ -31,9 +31,9 @@ TEST(TestRoboteqErrorFilter, InitialState)
 
 TEST(TestRoboteqErrorFilter, FilterError)
 {
-  using panther_hardware_interfaces::ErrorsFilterIds;
+  using husarion_ugv_hardware_interfaces::ErrorsFilterIds;
 
-  panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
+  husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
 
   roboteq_error_filter.UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true);
 
@@ -62,9 +62,9 @@ TEST(TestRoboteqErrorFilter, FilterError)
 
 TEST(TestRoboteqErrorFilter, Error)
 {
-  using panther_hardware_interfaces::ErrorsFilterIds;
+  using husarion_ugv_hardware_interfaces::ErrorsFilterIds;
 
-  panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
+  husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
 
   roboteq_error_filter.UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true);
 
@@ -85,9 +85,9 @@ TEST(TestRoboteqErrorFilter, Error)
 
 TEST(TestRoboteqErrorFilter, FilterSecondError)
 {
-  using panther_hardware_interfaces::ErrorsFilterIds;
+  using husarion_ugv_hardware_interfaces::ErrorsFilterIds;
 
-  panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
+  husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
 
   roboteq_error_filter.UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true);
 
@@ -116,9 +116,9 @@ TEST(TestRoboteqErrorFilter, FilterSecondError)
 
 TEST(TestRoboteqErrorFilter, SecondError)
 {
-  using panther_hardware_interfaces::ErrorsFilterIds;
+  using husarion_ugv_hardware_interfaces::ErrorsFilterIds;
 
-  panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
+  husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
 
   roboteq_error_filter.UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true);
 
@@ -139,9 +139,9 @@ TEST(TestRoboteqErrorFilter, SecondError)
 
 TEST(TestRoboteqErrorFilter, ErrorSingle)
 {
-  using panther_hardware_interfaces::ErrorsFilterIds;
+  using husarion_ugv_hardware_interfaces::ErrorsFilterIds;
 
-  panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
+  husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
 
   roboteq_error_filter.UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true);
 
@@ -154,9 +154,9 @@ TEST(TestRoboteqErrorFilter, ErrorSingle)
 
 TEST(TestRoboteqErrorFilter, ClearErrors)
 {
-  using panther_hardware_interfaces::ErrorsFilterIds;
+  using husarion_ugv_hardware_interfaces::ErrorsFilterIds;
 
-  panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
+  husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
 
   roboteq_error_filter.UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true);
   roboteq_error_filter.UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true);
@@ -187,9 +187,9 @@ TEST(TestRoboteqErrorFilter, ClearErrors)
 
 TEST(TestRoboteqErrorFilter, ClearErrorsCounters)
 {
-  using panther_hardware_interfaces::ErrorsFilterIds;
+  using husarion_ugv_hardware_interfaces::ErrorsFilterIds;
 
-  panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
+  husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1);
 
   roboteq_error_filter.UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true);
   roboteq_error_filter.UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true);
diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_robot_driver.cpp
similarity index 81%
rename from panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
rename to husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_robot_driver.cpp
index 714de2d9..26ee9120 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp
+++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_robot_driver.cpp
@@ -22,41 +22,43 @@
 
 #include <gmock/gmock.h>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/driver.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp"
 
 #include "utils/fake_can_socket.hpp"
 #include "utils/mock_driver.hpp"
 #include "utils/test_constants.hpp"
 
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
-class RoboteqRobotDriverWrapper : public panther_hardware_interfaces::RoboteqRobotDriver
+class RoboteqRobotDriverWrapper : public husarion_ugv_hardware_interfaces::RoboteqRobotDriver
 {
 public:
   RoboteqRobotDriverWrapper(
-    const panther_hardware_interfaces::CANopenSettings & canopen_settings,
-    const panther_hardware_interfaces::DrivetrainSettings & drivetrain_settings,
+    const husarion_ugv_hardware_interfaces::CANopenSettings & canopen_settings,
+    const husarion_ugv_hardware_interfaces::DrivetrainSettings & drivetrain_settings,
     const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000))
   : RoboteqRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time)
   {
     // Assume 2 drivers and 4 motor drivers
     mock_fl_motor_driver =
-      std::make_shared<panther_hardware_interfaces_test::MockMotorDriver::NiceMock>();
+      std::make_shared<husarion_ugv_hardware_interfaces_test::MockMotorDriver::NiceMock>();
     mock_fr_motor_driver =
-      std::make_shared<panther_hardware_interfaces_test::MockMotorDriver::NiceMock>();
+      std::make_shared<husarion_ugv_hardware_interfaces_test::MockMotorDriver::NiceMock>();
     mock_rl_motor_driver =
-      std::make_shared<panther_hardware_interfaces_test::MockMotorDriver::NiceMock>();
+      std::make_shared<husarion_ugv_hardware_interfaces_test::MockMotorDriver::NiceMock>();
     mock_rr_motor_driver =
-      std::make_shared<panther_hardware_interfaces_test::MockMotorDriver::NiceMock>();
+      std::make_shared<husarion_ugv_hardware_interfaces_test::MockMotorDriver::NiceMock>();
 
-    mock_front_driver = std::make_shared<panther_hardware_interfaces_test::MockDriver::NiceMock>();
+    mock_front_driver =
+      std::make_shared<husarion_ugv_hardware_interfaces_test::MockDriver::NiceMock>();
     mock_front_driver->AddMotorDriver(kLeftMotorDriverName, mock_fl_motor_driver);
     mock_front_driver->AddMotorDriver(kRightMotorDriverName, mock_fr_motor_driver);
 
-    mock_rear_driver = std::make_shared<panther_hardware_interfaces_test::MockDriver::NiceMock>();
+    mock_rear_driver =
+      std::make_shared<husarion_ugv_hardware_interfaces_test::MockDriver::NiceMock>();
     mock_rear_driver->AddMotorDriver(kLeftMotorDriverName, mock_rl_motor_driver);
     mock_rear_driver->AddMotorDriver(kRightMotorDriverName, mock_rr_motor_driver);
   }
@@ -75,12 +77,16 @@ class RoboteqRobotDriverWrapper : public panther_hardware_interfaces::RoboteqRob
   static constexpr char kLeftMotorDriverName[] = "left";
   static constexpr char kRightMotorDriverName[] = "right";
 
-  std::shared_ptr<panther_hardware_interfaces_test::MockDriver::NiceMock> mock_front_driver;
-  std::shared_ptr<panther_hardware_interfaces_test::MockDriver::NiceMock> mock_rear_driver;
-  std::shared_ptr<panther_hardware_interfaces_test::MockMotorDriver::NiceMock> mock_fl_motor_driver;
-  std::shared_ptr<panther_hardware_interfaces_test::MockMotorDriver::NiceMock> mock_fr_motor_driver;
-  std::shared_ptr<panther_hardware_interfaces_test::MockMotorDriver::NiceMock> mock_rl_motor_driver;
-  std::shared_ptr<panther_hardware_interfaces_test::MockMotorDriver::NiceMock> mock_rr_motor_driver;
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockDriver::NiceMock> mock_front_driver;
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockDriver::NiceMock> mock_rear_driver;
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockMotorDriver::NiceMock>
+    mock_fl_motor_driver;
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockMotorDriver::NiceMock>
+    mock_fr_motor_driver;
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockMotorDriver::NiceMock>
+    mock_rl_motor_driver;
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockMotorDriver::NiceMock>
+    mock_rr_motor_driver;
 };
 
 class TestRoboteqRobotDriverInitialization : public ::testing::Test
@@ -88,19 +94,19 @@ class TestRoboteqRobotDriverInitialization : public ::testing::Test
 public:
   TestRoboteqRobotDriverInitialization()
   {
-    can_socket_ = std::make_unique<panther_hardware_interfaces_test::FakeCANSocket>(
-      panther_hardware_interfaces_test::kCANopenSettings.can_interface_name);
+    can_socket_ = std::make_unique<husarion_ugv_hardware_interfaces_test::FakeCANSocket>(
+      husarion_ugv_hardware_interfaces_test::kCANopenSettings.can_interface_name);
     can_socket_->Initialize();
 
     robot_driver_ = std::make_unique<RoboteqRobotDriverWrapper>(
-      panther_hardware_interfaces_test::kCANopenSettings,
-      panther_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10));
+      husarion_ugv_hardware_interfaces_test::kCANopenSettings,
+      husarion_ugv_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10));
   }
 
   ~TestRoboteqRobotDriverInitialization() {}
 
 protected:
-  std::unique_ptr<panther_hardware_interfaces_test::FakeCANSocket> can_socket_;
+  std::unique_ptr<husarion_ugv_hardware_interfaces_test::FakeCANSocket> can_socket_;
   std::unique_ptr<RoboteqRobotDriverWrapper> robot_driver_;
 };
 
@@ -164,7 +170,7 @@ TEST_F(TestRoboteqRobotDriver, GetDataError)
   const std::string name = "invalid_name";
   const std::string error_msg = "Data with name '" + name + "' does not exist.";
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&] { robot_driver_->GetData(name); }, error_msg));
 }
 
@@ -206,10 +212,10 @@ TEST_F(TestRoboteqRobotDriver, UpdateCommunicationStateHeartbeatTimeout)
 
 TEST_F(TestRoboteqRobotDriver, UpdateMotorsState)
 {
-  using panther_hardware_interfaces::MotorChannels;
-  using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
-  using panther_hardware_interfaces_test::kRbtqPosFbToRad;
-  using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+  using husarion_ugv_hardware_interfaces::MotorChannels;
+  using husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  using husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad;
+  using husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
 
   const std::int32_t fl_pos = 101;
   const std::int32_t fl_vel = 102;
@@ -225,17 +231,17 @@ TEST_F(TestRoboteqRobotDriver, UpdateMotorsState)
   const std::int32_t rr_current = 403;
 
   ON_CALL(*robot_driver_->mock_fl_motor_driver, ReadState())
-    .WillByDefault(::testing::Return(
-      panther_hardware_interfaces::MotorDriverState({fl_pos, fl_vel, fl_current, {0, 0}, {0, 0}})));
+    .WillByDefault(::testing::Return(husarion_ugv_hardware_interfaces::MotorDriverState(
+      {fl_pos, fl_vel, fl_current, {0, 0}, {0, 0}})));
   ON_CALL(*robot_driver_->mock_fr_motor_driver, ReadState())
-    .WillByDefault(::testing::Return(
-      panther_hardware_interfaces::MotorDriverState({fr_pos, fr_vel, fr_current, {0, 0}, {0, 0}})));
+    .WillByDefault(::testing::Return(husarion_ugv_hardware_interfaces::MotorDriverState(
+      {fr_pos, fr_vel, fr_current, {0, 0}, {0, 0}})));
   ON_CALL(*robot_driver_->mock_rl_motor_driver, ReadState())
-    .WillByDefault(::testing::Return(
-      panther_hardware_interfaces::MotorDriverState({rl_pos, rl_vel, rl_current, {0, 0}, {0, 0}})));
+    .WillByDefault(::testing::Return(husarion_ugv_hardware_interfaces::MotorDriverState(
+      {rl_pos, rl_vel, rl_current, {0, 0}, {0, 0}})));
   ON_CALL(*robot_driver_->mock_rr_motor_driver, ReadState())
-    .WillByDefault(::testing::Return(
-      panther_hardware_interfaces::MotorDriverState({rr_pos, rr_vel, rr_current, {0, 0}, {0, 0}})));
+    .WillByDefault(::testing::Return(husarion_ugv_hardware_interfaces::MotorDriverState(
+      {rr_pos, rr_vel, rr_current, {0, 0}, {0, 0}})));
 
   robot_driver_->UpdateMotorsState();
 
@@ -268,10 +274,10 @@ TEST_F(TestRoboteqRobotDriver, UpdateMotorsState)
 TEST_F(TestRoboteqRobotDriver, UpdateMotorsStateTimestamps)
 {
   auto current_time = GetCurrentTimeWithTimeout(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms);
+    husarion_ugv_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms);
 
   auto read_motor_driver_state_method = [&current_time]() {
-    panther_hardware_interfaces::MotorDriverState state;
+    husarion_ugv_hardware_interfaces::MotorDriverState state;
     state.pos_timestamp = current_time;
     state.vel_current_timestamp = current_time;
     return state;
@@ -302,9 +308,9 @@ TEST_F(TestRoboteqRobotDriver, UpdateMotorsStateTimestamps)
 TEST_F(TestRoboteqRobotDriver, UpdateMotorsStateTimeout)
 {
   const auto current_time = GetCurrentTimeWithTimeout(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms);
+    husarion_ugv_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms);
 
-  panther_hardware_interfaces::MotorDriverState state = {0, 0, 0, current_time, current_time};
+  husarion_ugv_hardware_interfaces::MotorDriverState state = {0, 0, 0, current_time, current_time};
 
   ON_CALL(*robot_driver_->mock_fl_motor_driver, ReadState())
     .WillByDefault(::testing::Return(state));
@@ -327,7 +333,7 @@ TEST_F(TestRoboteqRobotDriver, UpdateMotorsStateTimeout)
 
 TEST_F(TestRoboteqRobotDriver, UpdateDriverState)
 {
-  using panther_hardware_interfaces::MotorChannels;
+  using husarion_ugv_hardware_interfaces::MotorChannels;
 
   const std::int16_t f_temp = 30;
   const std::int16_t r_temp = 32;
@@ -349,7 +355,7 @@ TEST_F(TestRoboteqRobotDriver, UpdateDriverState)
   const std::uint8_t runtime_error_forward_limit_triggered = static_cast<std::uint8_t>(0b10000);
   const std::uint8_t runtime_error_reverse_limit_triggered = static_cast<std::uint8_t>(0b100000);
 
-  panther_hardware_interfaces::DriverState front_driver_state_data = {
+  husarion_ugv_hardware_interfaces::DriverState front_driver_state_data = {
     fault_flag_overheat,
     script_flag_encoder_disconnected,
     runtime_error_loop_error,
@@ -362,7 +368,7 @@ TEST_F(TestRoboteqRobotDriver, UpdateDriverState)
     {0, 0},
     {0, 0}};
 
-  panther_hardware_interfaces::DriverState rear_driver_state_data = {
+  husarion_ugv_hardware_interfaces::DriverState rear_driver_state_data = {
     fault_flag_overvoltage,
     script_flag_amp_limiter,
     runtime_error_forward_limit_triggered,
@@ -377,10 +383,10 @@ TEST_F(TestRoboteqRobotDriver, UpdateDriverState)
 
   ON_CALL(*robot_driver_->mock_front_driver, ReadState())
     .WillByDefault(
-      ::testing::Return(panther_hardware_interfaces::DriverState(front_driver_state_data)));
+      ::testing::Return(husarion_ugv_hardware_interfaces::DriverState(front_driver_state_data)));
   ON_CALL(*robot_driver_->mock_rear_driver, ReadState())
     .WillByDefault(
-      ::testing::Return(panther_hardware_interfaces::DriverState(rear_driver_state_data)));
+      ::testing::Return(husarion_ugv_hardware_interfaces::DriverState(rear_driver_state_data)));
 
   robot_driver_->UpdateDriversState();
 
@@ -420,10 +426,10 @@ TEST_F(TestRoboteqRobotDriver, UpdateDriverState)
 TEST_F(TestRoboteqRobotDriver, UpdateDriverStateTimestamps)
 {
   auto current_time = GetCurrentTimeWithTimeout(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms);
+    husarion_ugv_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms);
 
   auto read_driver_state_method = [&current_time]() {
-    panther_hardware_interfaces::DriverState state;
+    husarion_ugv_hardware_interfaces::DriverState state;
     state.flags_current_timestamp = current_time;
     state.voltages_temps_timestamp = current_time;
     return state;
@@ -450,9 +456,9 @@ TEST_F(TestRoboteqRobotDriver, UpdateDriverStateTimestamps)
 TEST_F(TestRoboteqRobotDriver, UpdateDriverStateTimeout)
 {
   const auto current_time = GetCurrentTimeWithTimeout(
-    panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms);
+    husarion_ugv_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms);
 
-  panther_hardware_interfaces::DriverState state = {
+  husarion_ugv_hardware_interfaces::DriverState state = {
     0, 0, 0, 0, 0, 0, 0, 0, 0, current_time, current_time};
 
   ON_CALL(*robot_driver_->mock_front_driver, ReadState()).WillByDefault(::testing::Return(state));
diff --git a/panther_hardware_interfaces/test/unit/panther_system/test_lynx_system.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_lynx_system.cpp
similarity index 64%
rename from panther_hardware_interfaces/test/unit/panther_system/test_lynx_system.cpp
rename to husarion_ugv_hardware_interfaces/test/unit/robot_system/test_lynx_system.cpp
index ed1ecd65..66da3df6 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/test_lynx_system.cpp
+++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_lynx_system.cpp
@@ -22,22 +22,22 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include "panther_hardware_interfaces/panther_system/lynx_system.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/lynx_system.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp"
 
 #include "utils/system_test_utils.hpp"
 #include "utils/test_constants.hpp"
 
-class LynxSystemWrapper : public panther_hardware_interfaces::LynxSystem
+class LynxSystemWrapper : public husarion_ugv_hardware_interfaces::LynxSystem
 {
 public:
   LynxSystemWrapper() : LynxSystem()
   {
     mock_robot_driver =
-      std::make_shared<panther_hardware_interfaces_test::MockRobotDriver::NiceMock>();
+      std::make_shared<husarion_ugv_hardware_interfaces_test::MockRobotDriver::NiceMock>();
     mock_gpio_controller =
-      std::make_shared<panther_hardware_interfaces_test::MockGPIOController::NiceMock>();
-    mock_e_stop = std::make_shared<panther_hardware_interfaces_test::MockEStop::NiceMock>();
+      std::make_shared<husarion_ugv_hardware_interfaces_test::MockGPIOController::NiceMock>();
+    mock_e_stop = std::make_shared<husarion_ugv_hardware_interfaces_test::MockEStop::NiceMock>();
   }
 
   void DefineRobotDriver() override { robot_driver_ = mock_robot_driver; }
@@ -61,20 +61,24 @@ class LynxSystemWrapper : public panther_hardware_interfaces::LynxSystem
     hw_commands_velocities_[3] = velocities[3];
   }
 
-  panther_hardware_interfaces::CANopenSettings GetCANopenSettings() { return canopen_settings_; }
+  husarion_ugv_hardware_interfaces::CANopenSettings GetCANopenSettings()
+  {
+    return canopen_settings_;
+  }
   std::vector<double> GetHwStatesPositions() { return hw_states_positions_; }
   std::vector<double> GetHwStatesVelocities() { return hw_states_velocities_; }
   std::vector<double> GetHwStatesEfforts() { return hw_states_efforts_; }
 
-  std::shared_ptr<panther_hardware_interfaces::RoboteqErrorFilter> GetRoboteqErrorFilter()
+  std::shared_ptr<husarion_ugv_hardware_interfaces::RoboteqErrorFilter> GetRoboteqErrorFilter()
   {
     return roboteq_error_filter_;
   }
 
-  std::shared_ptr<panther_hardware_interfaces_test::MockRobotDriver::NiceMock> mock_robot_driver;
-  std::shared_ptr<panther_hardware_interfaces_test::MockGPIOController::NiceMock>
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockRobotDriver::NiceMock>
+    mock_robot_driver;
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockGPIOController::NiceMock>
     mock_gpio_controller;
-  std::shared_ptr<panther_hardware_interfaces_test::MockEStop::NiceMock> mock_e_stop;
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockEStop::NiceMock> mock_e_stop;
 };
 
 class TestLynxSystem : public ::testing::Test
@@ -84,7 +88,7 @@ class TestLynxSystem : public ::testing::Test
   {
     lynx_system_ = std::make_shared<LynxSystemWrapper>();
 
-    hardware_info_ = panther_hardware_interfaces_test::GenerateDefaultHardwareInfo();
+    hardware_info_ = husarion_ugv_hardware_interfaces_test::GenerateDefaultHardwareInfo();
     hardware_info_.hardware_parameters.emplace("driver_can_id", "1");
 
     lynx_system_->on_init(hardware_info_);
@@ -106,7 +110,7 @@ TEST_F(TestLynxSystem, ReadCANopenSettingsDriverCANIDs)
 
   EXPECT_EQ(canopen_settings.driver_can_ids.size(), 1);
   EXPECT_EQ(
-    canopen_settings.driver_can_ids.at(panther_hardware_interfaces::DriverNames::DEFAULT), 1);
+    canopen_settings.driver_can_ids.at(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT), 1);
 }
 
 TEST_F(TestLynxSystem, UpdateHwStates)
@@ -118,29 +122,31 @@ TEST_F(TestLynxSystem, UpdateHwStates)
   const std::int16_t right_vel = 50;
   const std::int16_t right_eff = 60;
 
-  const auto left_expected_pos = left_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad;
-  const auto left_expected_vel = left_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
-  const auto left_expected_eff = left_eff *
-                                 panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
-  const auto right_expected_pos = right_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad;
+  const auto left_expected_pos = left_pos * husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad;
+  const auto left_expected_vel = left_vel *
+                                 husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+  const auto left_expected_eff =
+    left_eff * husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  const auto right_expected_pos = right_pos *
+                                  husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad;
   const auto right_expected_vel = right_vel *
-                                  panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
-  const auto right_expected_eff = right_eff *
-                                  panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+                                  husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+  const auto right_expected_eff =
+    right_eff * husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
 
-  panther_hardware_interfaces::MotorDriverState left_motor_driver_state = {
+  husarion_ugv_hardware_interfaces::MotorDriverState left_motor_driver_state = {
     left_pos, left_vel, left_eff, {0, 0}, {0, 0}};
-  panther_hardware_interfaces::MotorDriverState right_motor_driver_state = {
+  husarion_ugv_hardware_interfaces::MotorDriverState right_motor_driver_state = {
     right_pos, right_vel, right_eff, {0, 0}, {0, 0}};
 
-  panther_hardware_interfaces::DriverData roboteq_data(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
+  husarion_ugv_hardware_interfaces::DriverData roboteq_data(
+    husarion_ugv_hardware_interfaces_test::kDrivetrainSettings);
   // left - channel 2, right - channel 1
   roboteq_data.SetMotorsStates(right_motor_driver_state, left_motor_driver_state, false);
 
   EXPECT_CALL(
     *lynx_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT)))
     .WillOnce(::testing::ReturnRef(roboteq_data));
 
   ASSERT_NO_THROW(lynx_system_->UpdateHwStates());
@@ -163,22 +169,22 @@ TEST_F(TestLynxSystem, UpdateHwStates)
 
 TEST_F(TestLynxSystem, UpdateMotorsStateDataTimedOut)
 {
-  panther_hardware_interfaces::MotorDriverState motor_driver_state;
+  husarion_ugv_hardware_interfaces::MotorDriverState motor_driver_state;
 
-  panther_hardware_interfaces::DriverData roboteq_data(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
+  husarion_ugv_hardware_interfaces::DriverData roboteq_data(
+    husarion_ugv_hardware_interfaces_test::kDrivetrainSettings);
   roboteq_data.SetMotorsStates(motor_driver_state, motor_driver_state, true);
 
   EXPECT_CALL(
     *lynx_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT)))
     .WillOnce(::testing::ReturnRef(roboteq_data));
 
   lynx_system_->UpdateMotorsStateDataTimedOut();
 
   auto error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap();
-  auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
-    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES));
+  auto error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at(
+    husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES));
 
   EXPECT_TRUE(error);
 
@@ -187,15 +193,15 @@ TEST_F(TestLynxSystem, UpdateMotorsStateDataTimedOut)
 
   EXPECT_CALL(
     *lynx_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT)))
     .WillOnce(::testing::ReturnRef(roboteq_data));
 
   lynx_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag();
   lynx_system_->UpdateMotorsStateDataTimedOut();
 
   error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap();
-  error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
-    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES));
+  error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at(
+    husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES));
 
   EXPECT_FALSE(error);
 }
@@ -208,23 +214,23 @@ TEST_F(TestLynxSystem, UpdateDriverStateMsg)
 
 TEST_F(TestLynxSystem, UpdateFlagErrors)
 {
-  auto driver_state = panther_hardware_interfaces::DriverState();
+  auto driver_state = husarion_ugv_hardware_interfaces::DriverState();
   driver_state.fault_flags = 0b01;
 
-  panther_hardware_interfaces::DriverData roboteq_data(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
+  husarion_ugv_hardware_interfaces::DriverData roboteq_data(
+    husarion_ugv_hardware_interfaces_test::kDrivetrainSettings);
   roboteq_data.SetDriverState(driver_state, false);
 
   EXPECT_CALL(
     *lynx_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT)))
     .WillOnce(::testing::ReturnRef(roboteq_data));
 
   lynx_system_->UpdateFlagErrors();
 
   auto error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap();
-  auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
-    panther_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER));
+  auto error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at(
+    husarion_ugv_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER));
 
   EXPECT_TRUE(error);
 
@@ -234,37 +240,37 @@ TEST_F(TestLynxSystem, UpdateFlagErrors)
 
   EXPECT_CALL(
     *lynx_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT)))
     .WillOnce(::testing::ReturnRef(roboteq_data));
 
   lynx_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag();
   lynx_system_->UpdateFlagErrors();
 
   error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap();
-  error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
-    panther_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER));
+  error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at(
+    husarion_ugv_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER));
 
   EXPECT_FALSE(error);
 }
 
 TEST_F(TestLynxSystem, UpdateDriverStateDataTimedOut)
 {
-  panther_hardware_interfaces::DriverState driver_state;
+  husarion_ugv_hardware_interfaces::DriverState driver_state;
 
-  panther_hardware_interfaces::DriverData roboteq_data(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
+  husarion_ugv_hardware_interfaces::DriverData roboteq_data(
+    husarion_ugv_hardware_interfaces_test::kDrivetrainSettings);
   roboteq_data.SetDriverState(driver_state, true);
 
   EXPECT_CALL(
     *lynx_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT)))
     .WillOnce(::testing::ReturnRef(roboteq_data));
 
   lynx_system_->UpdateDriverStateDataTimedOut();
 
   auto error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap();
-  auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
-    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE));
+  auto error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at(
+    husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE));
 
   EXPECT_TRUE(error);
 
@@ -273,15 +279,15 @@ TEST_F(TestLynxSystem, UpdateDriverStateDataTimedOut)
 
   EXPECT_CALL(
     *lynx_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT)))
     .WillOnce(::testing::ReturnRef(roboteq_data));
 
   lynx_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag();
   lynx_system_->UpdateDriverStateDataTimedOut();
 
   error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap();
-  error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
-    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE));
+  error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at(
+    husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE));
 
   EXPECT_FALSE(error);
 }
diff --git a/panther_hardware_interfaces/test/unit/panther_system/test_panther_system.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_panther_system.cpp
similarity index 59%
rename from panther_hardware_interfaces/test/unit/panther_system/test_panther_system.cpp
rename to husarion_ugv_hardware_interfaces/test/unit/robot_system/test_panther_system.cpp
index f75704ef..057b4b5f 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/test_panther_system.cpp
+++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_panther_system.cpp
@@ -22,28 +22,30 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include "panther_hardware_interfaces/panther_system/panther_system.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/panther_system.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp"
 
 #include "utils/system_test_utils.hpp"
 #include "utils/test_constants.hpp"
 
-class PantherSystemWrapper : public panther_hardware_interfaces::PantherSystem
+class PantherSystemWrapper : public husarion_ugv_hardware_interfaces::PantherSystem
 {
 public:
   PantherSystemWrapper() : PantherSystem()
   {
     mock_robot_driver =
-      std::make_shared<panther_hardware_interfaces_test::MockRobotDriver::NiceMock>();
+      std::make_shared<husarion_ugv_hardware_interfaces_test::MockRobotDriver::NiceMock>();
     mock_gpio_controller =
-      std::make_shared<panther_hardware_interfaces_test::MockGPIOController::NiceMock>();
-    mock_e_stop = std::make_shared<panther_hardware_interfaces_test::MockEStop::NiceMock>();
+      std::make_shared<husarion_ugv_hardware_interfaces_test::MockGPIOController::NiceMock>();
+    mock_e_stop = std::make_shared<husarion_ugv_hardware_interfaces_test::MockEStop::NiceMock>();
 
     ON_CALL(
-      *mock_robot_driver, GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+      *mock_robot_driver,
+      GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT)))
       .WillByDefault(::testing::ReturnRef(default_driver_data));
     ON_CALL(
-      *mock_robot_driver, GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+      *mock_robot_driver,
+      GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR)))
       .WillByDefault(::testing::ReturnRef(default_driver_data));
   }
 
@@ -68,24 +70,29 @@ class PantherSystemWrapper : public panther_hardware_interfaces::PantherSystem
     hw_commands_velocities_[3] = velocities[3];
   }
 
-  panther_hardware_interfaces::CANopenSettings GetCANopenSettings() { return canopen_settings_; }
+  husarion_ugv_hardware_interfaces::CANopenSettings GetCANopenSettings()
+  {
+    return canopen_settings_;
+  }
   std::vector<double> GetHwStatesPositions() { return hw_states_positions_; }
   std::vector<double> GetHwStatesVelocities() { return hw_states_velocities_; }
   std::vector<double> GetHwStatesEfforts() { return hw_states_efforts_; }
 
-  std::shared_ptr<panther_hardware_interfaces::RoboteqErrorFilter> GetRoboteqErrorFilter()
+  std::shared_ptr<husarion_ugv_hardware_interfaces::RoboteqErrorFilter> GetRoboteqErrorFilter()
   {
     return roboteq_error_filter_;
   }
 
-  std::shared_ptr<panther_hardware_interfaces_test::MockRobotDriver::NiceMock> mock_robot_driver;
-  std::shared_ptr<panther_hardware_interfaces_test::MockGPIOController::NiceMock>
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockRobotDriver::NiceMock>
+    mock_robot_driver;
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockGPIOController::NiceMock>
     mock_gpio_controller;
-  std::shared_ptr<panther_hardware_interfaces_test::MockEStop::NiceMock> mock_e_stop;
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockEStop::NiceMock> mock_e_stop;
 
 private:
-  panther_hardware_interfaces::DriverData default_driver_data =
-    panther_hardware_interfaces::DriverData(panther_hardware_interfaces_test::kDrivetrainSettings);
+  husarion_ugv_hardware_interfaces::DriverData default_driver_data =
+    husarion_ugv_hardware_interfaces::DriverData(
+      husarion_ugv_hardware_interfaces_test::kDrivetrainSettings);
 };
 
 class TestPantherSystem : public ::testing::Test
@@ -95,7 +102,7 @@ class TestPantherSystem : public ::testing::Test
   {
     panther_system_ = std::make_shared<PantherSystemWrapper>();
 
-    hardware_info_ = panther_hardware_interfaces_test::GenerateDefaultHardwareInfo();
+    hardware_info_ = husarion_ugv_hardware_interfaces_test::GenerateDefaultHardwareInfo();
     hardware_info_.hardware_parameters.emplace("front_driver_can_id", "1");
     hardware_info_.hardware_parameters.emplace("rear_driver_can_id", "2");
 
@@ -117,8 +124,10 @@ TEST_F(TestPantherSystem, ReadCANopenSettingsDriverCANIDs)
   const auto canopen_settings = panther_system_->GetCANopenSettings();
 
   EXPECT_EQ(canopen_settings.driver_can_ids.size(), 2);
-  EXPECT_EQ(canopen_settings.driver_can_ids.at(panther_hardware_interfaces::DriverNames::FRONT), 1);
-  EXPECT_EQ(canopen_settings.driver_can_ids.at(panther_hardware_interfaces::DriverNames::REAR), 2);
+  EXPECT_EQ(
+    canopen_settings.driver_can_ids.at(husarion_ugv_hardware_interfaces::DriverNames::FRONT), 1);
+  EXPECT_EQ(
+    canopen_settings.driver_can_ids.at(husarion_ugv_hardware_interfaces::DriverNames::REAR), 2);
 }
 
 TEST_F(TestPantherSystem, UpdateHwStates)
@@ -136,47 +145,51 @@ TEST_F(TestPantherSystem, UpdateHwStates)
   const std::int16_t rr_vel = 110;
   const std::int16_t rr_eff = 120;
 
-  const auto fl_expected_pos = fl_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad;
-  const auto fl_expected_vel = fl_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+  const auto fl_expected_pos = fl_pos * husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad;
+  const auto fl_expected_vel = fl_vel *
+                               husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
   const auto fl_expected_eff = fl_eff *
-                               panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
-  const auto fr_expected_pos = fr_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad;
-  const auto fr_expected_vel = fr_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+                               husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  const auto fr_expected_pos = fr_pos * husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad;
+  const auto fr_expected_vel = fr_vel *
+                               husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
   const auto fr_expected_eff = fr_eff *
-                               panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
-  const auto rl_expected_pos = rl_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad;
-  const auto rl_expected_vel = rl_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+                               husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  const auto rl_expected_pos = rl_pos * husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad;
+  const auto rl_expected_vel = rl_vel *
+                               husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
   const auto rl_expected_eff = rl_eff *
-                               panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
-  const auto rr_expected_pos = rr_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad;
-  const auto rr_expected_vel = rr_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
+                               husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+  const auto rr_expected_pos = rr_pos * husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad;
+  const auto rr_expected_vel = rr_vel *
+                               husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec;
   const auto rr_expected_eff = rr_eff *
-                               panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
+                               husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters;
 
-  panther_hardware_interfaces::MotorDriverState fl_motor_driver_state = {
+  husarion_ugv_hardware_interfaces::MotorDriverState fl_motor_driver_state = {
     fl_pos, fl_vel, fl_eff, {0, 0}, {0, 0}};
-  panther_hardware_interfaces::MotorDriverState fr_motor_driver_state = {
+  husarion_ugv_hardware_interfaces::MotorDriverState fr_motor_driver_state = {
     fr_pos, fr_vel, fr_eff, {0, 0}, {0, 0}};
-  panther_hardware_interfaces::MotorDriverState rl_motor_driver_state = {
+  husarion_ugv_hardware_interfaces::MotorDriverState rl_motor_driver_state = {
     rl_pos, rl_vel, rl_eff, {0, 0}, {0, 0}};
-  panther_hardware_interfaces::MotorDriverState rr_motor_driver_state = {
+  husarion_ugv_hardware_interfaces::MotorDriverState rr_motor_driver_state = {
     rr_pos, rr_vel, rr_eff, {0, 0}, {0, 0}};
 
-  panther_hardware_interfaces::DriverData front_roboteq_data(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
-  panther_hardware_interfaces::DriverData rear_roboteq_data(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
+  husarion_ugv_hardware_interfaces::DriverData front_roboteq_data(
+    husarion_ugv_hardware_interfaces_test::kDrivetrainSettings);
+  husarion_ugv_hardware_interfaces::DriverData rear_roboteq_data(
+    husarion_ugv_hardware_interfaces_test::kDrivetrainSettings);
   // left - channel 2, right - channel 1
   front_roboteq_data.SetMotorsStates(fr_motor_driver_state, fl_motor_driver_state, false);
   rear_roboteq_data.SetMotorsStates(rr_motor_driver_state, rl_motor_driver_state, false);
 
   EXPECT_CALL(
     *panther_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT)))
     .WillOnce(::testing::ReturnRef(front_roboteq_data));
   EXPECT_CALL(
     *panther_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR)))
     .WillOnce(::testing::ReturnRef(rear_roboteq_data));
 
   ASSERT_NO_THROW(panther_system_->UpdateHwStates());
@@ -199,26 +212,26 @@ TEST_F(TestPantherSystem, UpdateHwStates)
 
 TEST_F(TestPantherSystem, UpdateMotorsStateDataTimedOut)
 {
-  panther_hardware_interfaces::MotorDriverState motor_driver_state;
+  husarion_ugv_hardware_interfaces::MotorDriverState motor_driver_state;
 
-  panther_hardware_interfaces::DriverData roboteq_data(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
+  husarion_ugv_hardware_interfaces::DriverData roboteq_data(
+    husarion_ugv_hardware_interfaces_test::kDrivetrainSettings);
   roboteq_data.SetMotorsStates(motor_driver_state, motor_driver_state, true);
 
   EXPECT_CALL(
     *panther_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT)))
     .WillOnce(::testing::ReturnRef(roboteq_data));
   EXPECT_CALL(
     *panther_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR)))
     .Times(0);
 
   panther_system_->UpdateMotorsStateDataTimedOut();
 
   auto error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap();
-  auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
-    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES));
+  auto error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at(
+    husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES));
 
   EXPECT_TRUE(error);
 
@@ -227,19 +240,19 @@ TEST_F(TestPantherSystem, UpdateMotorsStateDataTimedOut)
 
   EXPECT_CALL(
     *panther_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT)))
     .Times(1);
   EXPECT_CALL(
     *panther_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR)))
     .Times(1);
 
   panther_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag();
   panther_system_->UpdateMotorsStateDataTimedOut();
 
   error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap();
-  error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
-    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES));
+  error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at(
+    husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES));
 
   EXPECT_FALSE(error);
 }
@@ -252,30 +265,30 @@ TEST_F(TestPantherSystem, UpdateDriverStateMsg)
 
 TEST_F(TestPantherSystem, UpdateFlagErrors)
 {
-  panther_hardware_interfaces::DriverState driver_state;
+  husarion_ugv_hardware_interfaces::DriverState driver_state;
   driver_state.fault_flags = 0b01;
   driver_state.script_flags = 0;
   driver_state.runtime_stat_flag_channel_1 = 0;
   driver_state.runtime_stat_flag_channel_2 = 0;
 
-  panther_hardware_interfaces::DriverData roboteq_data(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
+  husarion_ugv_hardware_interfaces::DriverData roboteq_data(
+    husarion_ugv_hardware_interfaces_test::kDrivetrainSettings);
   roboteq_data.SetDriverState(driver_state, false);
 
   EXPECT_CALL(
     *panther_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT)))
     .Times(1);
   EXPECT_CALL(
     *panther_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR)))
     .WillOnce(::testing::ReturnRef(roboteq_data));
 
   panther_system_->UpdateFlagErrors();
 
   auto error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap();
-  auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
-    panther_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER));
+  auto error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at(
+    husarion_ugv_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER));
 
   EXPECT_TRUE(error);
 
@@ -285,45 +298,45 @@ TEST_F(TestPantherSystem, UpdateFlagErrors)
 
   EXPECT_CALL(
     *panther_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT)))
     .Times(1);
   EXPECT_CALL(
     *panther_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR)))
     .WillOnce(::testing::ReturnRef(roboteq_data));
 
   panther_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag();
   panther_system_->UpdateFlagErrors();
 
   error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap();
-  error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
-    panther_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER));
+  error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at(
+    husarion_ugv_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER));
 
   EXPECT_FALSE(error);
 }
 
 TEST_F(TestPantherSystem, UpdateDriverStateDataTimedOut)
 {
-  panther_hardware_interfaces::DriverState driver_state;
+  husarion_ugv_hardware_interfaces::DriverState driver_state;
 
-  panther_hardware_interfaces::DriverData roboteq_data(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
+  husarion_ugv_hardware_interfaces::DriverData roboteq_data(
+    husarion_ugv_hardware_interfaces_test::kDrivetrainSettings);
   roboteq_data.SetDriverState(driver_state, true);
 
   EXPECT_CALL(
     *panther_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT)))
     .WillOnce(::testing::ReturnRef(roboteq_data));
   EXPECT_CALL(
     *panther_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR)))
     .Times(0);
 
   panther_system_->UpdateDriverStateDataTimedOut();
 
   auto error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap();
-  auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
-    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE));
+  auto error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at(
+    husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE));
 
   EXPECT_TRUE(error);
 
@@ -332,19 +345,19 @@ TEST_F(TestPantherSystem, UpdateDriverStateDataTimedOut)
 
   EXPECT_CALL(
     *panther_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT)))
     .WillOnce(::testing::ReturnRef(roboteq_data));
   EXPECT_CALL(
     *panther_system_->mock_robot_driver,
-    GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR)))
+    GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR)))
     .Times(1);
 
   panther_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag();
   panther_system_->UpdateDriverStateDataTimedOut();
 
   error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap();
-  error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at(
-    panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE));
+  error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at(
+    husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE));
 
   EXPECT_FALSE(error);
 }
diff --git a/panther_hardware_interfaces/test/unit/panther_system/test_system_ros_interface.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_system_ros_interface.cpp
similarity index 87%
rename from panther_hardware_interfaces/test/unit/panther_system/test_system_ros_interface.cpp
rename to husarion_ugv_hardware_interfaces/test/unit/robot_system/test_system_ros_interface.cpp
index 68e2fbff..6e3d15a1 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/test_system_ros_interface.cpp
+++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_system_ros_interface.cpp
@@ -22,13 +22,13 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <panther_hardware_interfaces/panther_system/system_ros_interface.hpp>
+#include <husarion_ugv_hardware_interfaces/robot_system/system_ros_interface.hpp>
 
 #include "utils/test_constants.hpp"
 
 using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState;
 
-class SystemROSInterfaceWrapper : public panther_hardware_interfaces::SystemROSInterface
+class SystemROSInterfaceWrapper : public husarion_ugv_hardware_interfaces::SystemROSInterface
 {
 public:
   SystemROSInterfaceWrapper(const std::string & node_name) : SystemROSInterface(node_name) {}
@@ -52,7 +52,7 @@ class TestSystemROSInterface : public ::testing::Test
 
 TEST(TestSystemROSInterfaceInitialization, NodeCreation)
 {
-  using panther_hardware_interfaces::SystemROSInterface;
+  using husarion_ugv_hardware_interfaces::SystemROSInterface;
 
   std::vector<std::string> node_names;
   const std::string system_node_name = "hardware_controller";
@@ -82,10 +82,10 @@ TEST(TestSystemROSInterfaceInitialization, NodeCreation)
 
 TEST_F(TestSystemROSInterface, UpdateMsgErrorFlags)
 {
-  panther_hardware_interfaces::DriverData data(
-    panther_hardware_interfaces_test::kDrivetrainSettings);
+  husarion_ugv_hardware_interfaces::DriverData data(
+    husarion_ugv_hardware_interfaces_test::kDrivetrainSettings);
 
-  panther_hardware_interfaces::DriverState driver_state;
+  husarion_ugv_hardware_interfaces::DriverState driver_state;
   driver_state.fault_flags = 0b00000001;
   driver_state.script_flags = 0b00000010;
   driver_state.runtime_stat_flag_channel_1 = 0b00001000;
@@ -106,7 +106,7 @@ TEST_F(TestSystemROSInterface, UpdateMsgErrorFlags)
 
 TEST_F(TestSystemROSInterface, UpdateMsgDriversStates)
 {
-  panther_hardware_interfaces::RoboteqDriverState state;
+  husarion_ugv_hardware_interfaces::RoboteqDriverState state;
 
   const std::int16_t temp = 36;
   const std::int16_t heatsink_temp = 37;
@@ -138,14 +138,14 @@ TEST_F(TestSystemROSInterface, UpdateMsgDriversStates)
 
 TEST_F(TestSystemROSInterface, UpdateMsgErrors)
 {
-  panther_hardware_interfaces::CANErrors can_errors;
+  husarion_ugv_hardware_interfaces::CANErrors can_errors;
   can_errors.error = true;
 
   can_errors.write_pdo_cmds_error = true;
   can_errors.read_pdo_motor_states_error = false;
   can_errors.read_pdo_driver_state_error = false;
 
-  panther_hardware_interfaces::DriverCANErrors driver_can_errors;
+  husarion_ugv_hardware_interfaces::DriverCANErrors driver_can_errors;
   driver_can_errors.motor_states_data_timed_out = true;
   driver_can_errors.driver_state_data_timed_out = false;
   driver_can_errors.can_error = false;
@@ -181,12 +181,13 @@ TEST_F(TestSystemROSInterface, CreateDriverStateEntryInMsg)
 
   // check 3 different methods that can create a new entry in the message
   system_ros_interface_->UpdateMsgErrorFlags(
-    driver_1_name,
-    panther_hardware_interfaces::DriverData(panther_hardware_interfaces_test::kDrivetrainSettings));
+    driver_1_name, husarion_ugv_hardware_interfaces::DriverData(
+                     husarion_ugv_hardware_interfaces_test::kDrivetrainSettings));
   system_ros_interface_->UpdateMsgDriversStates(driver_2_name, {});
 
-  panther_hardware_interfaces::CANErrors can_errors;
-  can_errors.driver_errors.emplace(driver_3_name, panther_hardware_interfaces::DriverCANErrors());
+  husarion_ugv_hardware_interfaces::CANErrors can_errors;
+  can_errors.driver_errors.emplace(
+    driver_3_name, husarion_ugv_hardware_interfaces::DriverCANErrors());
   system_ros_interface_->UpdateMsgErrors(can_errors);
 
   EXPECT_EQ(driver_state_msg.driver_states.size(), 3);
diff --git a/panther_hardware_interfaces/test/unit/panther_system/test_ugv_system.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_ugv_system.cpp
similarity index 90%
rename from panther_hardware_interfaces/test/unit/panther_system/test_ugv_system.cpp
rename to husarion_ugv_hardware_interfaces/test/unit/robot_system/test_ugv_system.cpp
index 0474f8cd..3170ae53 100644
--- a/panther_hardware_interfaces/test/unit/panther_system/test_ugv_system.cpp
+++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_ugv_system.cpp
@@ -23,21 +23,21 @@
 #include <hardware_interface/types/hardware_interface_type_values.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include "panther_hardware_interfaces/panther_system/ugv_system.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp"
 
 #include "utils/system_test_utils.hpp"
 
-class MockUGVSystem : public panther_hardware_interfaces::UGVSystem
+class MockUGVSystem : public husarion_ugv_hardware_interfaces::UGVSystem
 {
 public:
   MockUGVSystem(const std::vector<std::string> & joint_order)
-  : panther_hardware_interfaces::UGVSystem(joint_order)
+  : husarion_ugv_hardware_interfaces::UGVSystem(joint_order)
   {
     mock_robot_driver_ =
-      std::make_shared<panther_hardware_interfaces_test::MockRobotDriver::NiceMock>();
+      std::make_shared<husarion_ugv_hardware_interfaces_test::MockRobotDriver::NiceMock>();
     mock_gpio_controller_ =
-      std::make_shared<panther_hardware_interfaces_test::MockGPIOController::NiceMock>();
-    mock_e_stop_ = std::make_shared<panther_hardware_interfaces_test::MockEStop::NiceMock>();
+      std::make_shared<husarion_ugv_hardware_interfaces_test::MockGPIOController::NiceMock>();
+    mock_e_stop_ = std::make_shared<husarion_ugv_hardware_interfaces_test::MockEStop::NiceMock>();
 
     ON_CALL(*this, DefineRobotDriver()).WillByDefault(::testing::Invoke([&]() {
       robot_driver_ = mock_robot_driver_;
@@ -69,7 +69,8 @@ class MockUGVSystem : public panther_hardware_interfaces::UGVSystem
 
   void DefaultDefineRobotDriver()
   {
-    robot_driver_ = std::make_shared<panther_hardware_interfaces_test::MockRobotDriver::NiceMock>();
+    robot_driver_ =
+      std::make_shared<husarion_ugv_hardware_interfaces_test::MockRobotDriver::NiceMock>();
   }
 
   void SetHwCommandVelocity(const std::vector<double> & velocity)
@@ -83,18 +84,19 @@ class MockUGVSystem : public panther_hardware_interfaces::UGVSystem
   }
   void SetHwStateEffort(const std::vector<double> & effort) { hw_states_efforts_ = effort; }
 
-  std::shared_ptr<panther_hardware_interfaces_test::MockRobotDriver::NiceMock> GetMockRobotDriver()
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockRobotDriver::NiceMock>
+  GetMockRobotDriver()
   {
     return mock_robot_driver_;
   }
 
-  std::shared_ptr<panther_hardware_interfaces_test::MockGPIOController::NiceMock>
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockGPIOController::NiceMock>
   GetMockGPIOController()
   {
     return mock_gpio_controller_;
   }
 
-  std::shared_ptr<panther_hardware_interfaces_test::MockEStop::NiceMock> GetMockEStop()
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockEStop::NiceMock> GetMockEStop()
   {
     return mock_e_stop_;
   }
@@ -102,10 +104,11 @@ class MockUGVSystem : public panther_hardware_interfaces::UGVSystem
   using NiceMock = testing::NiceMock<MockUGVSystem>;
 
 private:
-  std::shared_ptr<panther_hardware_interfaces_test::MockRobotDriver::NiceMock> mock_robot_driver_;
-  std::shared_ptr<panther_hardware_interfaces_test::MockGPIOController::NiceMock>
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockRobotDriver::NiceMock>
+    mock_robot_driver_;
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockGPIOController::NiceMock>
     mock_gpio_controller_;
-  std::shared_ptr<panther_hardware_interfaces_test::MockEStop::NiceMock> mock_e_stop_;
+  std::shared_ptr<husarion_ugv_hardware_interfaces_test::MockEStop::NiceMock> mock_e_stop_;
 };
 
 class TestUGVSystem : public ::testing::Test
@@ -116,7 +119,7 @@ class TestUGVSystem : public ::testing::Test
     ugv_system_ =
       std::make_shared<MockUGVSystem::NiceMock>(std::vector<std::string>{"fl", "fr", "rl", "rr"});
 
-    hardware_info_ = panther_hardware_interfaces_test::GenerateDefaultHardwareInfo();
+    hardware_info_ = husarion_ugv_hardware_interfaces_test::GenerateDefaultHardwareInfo();
   }
 
   ~TestUGVSystem() {}
diff --git a/panther_hardware_interfaces/test/utils/fake_can_socket.hpp b/husarion_ugv_hardware_interfaces/test/utils/fake_can_socket.hpp
similarity index 86%
rename from panther_hardware_interfaces/test/utils/fake_can_socket.hpp
rename to husarion_ugv_hardware_interfaces/test/utils/fake_can_socket.hpp
index e2920cff..0cb07b86 100644
--- a/panther_hardware_interfaces/test/utils/fake_can_socket.hpp
+++ b/husarion_ugv_hardware_interfaces/test/utils/fake_can_socket.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_
-#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_
 
 #include <stdexcept>
 #include <string>
 
 #include <sys/socket.h>
 
-namespace panther_hardware_interfaces_test
+namespace husarion_ugv_hardware_interfaces_test
 {
 
 class FakeCANSocket
@@ -75,6 +75,6 @@ class FakeCANSocket
   bool can_device_created_;
 };
 
-}  // namespace panther_hardware_interfaces_test
+}  // namespace husarion_ugv_hardware_interfaces_test
 
-#endif  // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_
diff --git a/panther_hardware_interfaces/test/utils/mock_driver.hpp b/husarion_ugv_hardware_interfaces/test/utils/mock_driver.hpp
similarity index 63%
rename from panther_hardware_interfaces/test/utils/mock_driver.hpp
rename to husarion_ugv_hardware_interfaces/test/utils/mock_driver.hpp
index da2b3390..10f11674 100644
--- a/panther_hardware_interfaces/test/utils/mock_driver.hpp
+++ b/husarion_ugv_hardware_interfaces/test/utils/mock_driver.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_TEST_UTILS_MOCK_DRIVER_HPP_
-#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_MOCK_DRIVER_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_MOCK_DRIVER_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_MOCK_DRIVER_HPP_
 
 #include <cstdint>
 #include <future>
@@ -22,12 +22,12 @@
 
 #include <gmock/gmock.h>
 
-#include "panther_hardware_interfaces/panther_system/robot_driver/driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/driver.hpp"
 
-namespace panther_hardware_interfaces_test
+namespace husarion_ugv_hardware_interfaces_test
 {
 
-class MockDriver : public panther_hardware_interfaces::DriverInterface
+class MockDriver : public husarion_ugv_hardware_interfaces::DriverInterface
 {
 public:
   MockDriver()
@@ -43,12 +43,12 @@ class MockDriver : public panther_hardware_interfaces::DriverInterface
   MOCK_METHOD(bool, IsCANError, (), (const, override));
   MOCK_METHOD(bool, IsHeartbeatTimeout, (), (const, override));
 
-  MOCK_METHOD(panther_hardware_interfaces::DriverState, ReadState, (), (override));
+  MOCK_METHOD(husarion_ugv_hardware_interfaces::DriverState, ReadState, (), (override));
   MOCK_METHOD(void, ResetScript, (), (override));
   MOCK_METHOD(void, TurnOnEStop, (), (override));
   MOCK_METHOD(void, TurnOffEStop, (), (override));
 
-  std::shared_ptr<panther_hardware_interfaces::MotorDriverInterface> GetMotorDriver(
+  std::shared_ptr<husarion_ugv_hardware_interfaces::MotorDriverInterface> GetMotorDriver(
     const std::string & name) override
   {
     return motor_drivers_.at(name);
@@ -56,7 +56,7 @@ class MockDriver : public panther_hardware_interfaces::DriverInterface
 
   void AddMotorDriver(
     const std::string name,
-    std::shared_ptr<panther_hardware_interfaces::MotorDriverInterface> motor_driver) override
+    std::shared_ptr<husarion_ugv_hardware_interfaces::MotorDriverInterface> motor_driver) override
   {
     motor_drivers_.emplace(name, motor_driver);
   }
@@ -64,20 +64,20 @@ class MockDriver : public panther_hardware_interfaces::DriverInterface
   using NiceMock = testing::NiceMock<MockDriver>;
 
 private:
-  std::map<std::string, std::shared_ptr<panther_hardware_interfaces::MotorDriverInterface>>
+  std::map<std::string, std::shared_ptr<husarion_ugv_hardware_interfaces::MotorDriverInterface>>
     motor_drivers_;
 };
 
-class MockMotorDriver : public panther_hardware_interfaces::MotorDriverInterface
+class MockMotorDriver : public husarion_ugv_hardware_interfaces::MotorDriverInterface
 {
 public:
-  MOCK_METHOD(panther_hardware_interfaces::MotorDriverState, ReadState, (), (override));
+  MOCK_METHOD(husarion_ugv_hardware_interfaces::MotorDriverState, ReadState, (), (override));
   MOCK_METHOD(void, SendCmdVel, (const std::int32_t cmd), (override));
   MOCK_METHOD(void, TurnOnSafetyStop, (), (override));
 
   using NiceMock = testing::NiceMock<MockMotorDriver>;
 };
 
-}  // namespace panther_hardware_interfaces_test
+}  // namespace husarion_ugv_hardware_interfaces_test
 
-#endif  // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_MOCK_DRIVER_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_MOCK_DRIVER_HPP_
diff --git a/panther_hardware_interfaces/test/utils/mock_roboteq.hpp b/husarion_ugv_hardware_interfaces/test/utils/mock_roboteq.hpp
similarity index 95%
rename from panther_hardware_interfaces/test/utils/mock_roboteq.hpp
rename to husarion_ugv_hardware_interfaces/test/utils/mock_roboteq.hpp
index f6607813..763813f9 100644
--- a/panther_hardware_interfaces/test/utils/mock_roboteq.hpp
+++ b/husarion_ugv_hardware_interfaces/test/utils/mock_roboteq.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_
-#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_
 
 #include <atomic>
 #include <condition_variable>
@@ -31,7 +31,7 @@
 #include "lely/io2/sys/sigset.hpp"
 #include "lely/io2/sys/timer.hpp"
 
-namespace panther_hardware_interfaces_test
+namespace husarion_ugv_hardware_interfaces_test
 {
 
 enum class DriverFaultFlags {
@@ -284,11 +284,11 @@ class MockRoboteq
       std::thread([this, motors_states_period, driver_state_period]() {
         std::string slave_eds_path =
           std::filesystem::path(
-            ament_index_cpp::get_package_share_directory("panther_hardware_interfaces")) /
+            ament_index_cpp::get_package_share_directory("husarion_ugv_hardware_interfaces")) /
           "config" / "roboteq_motor_controllers_v80_21a.eds";
         std::string slave1_eds_bin_path =
           std::filesystem::path(
-            ament_index_cpp::get_package_share_directory("panther_hardware_interfaces")) /
+            ament_index_cpp::get_package_share_directory("husarion_ugv_hardware_interfaces")) /
           "test" / "config" / "slave_1.bin";
 
         lely::io::IoGuard io_guard;
@@ -359,6 +359,6 @@ class MockRoboteq
   std::shared_ptr<RoboteqSlave> driver_;
 };
 
-}  // namespace panther_hardware_interfaces_test
+}  // namespace husarion_ugv_hardware_interfaces_test
 
-#endif  // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_
diff --git a/panther_hardware_interfaces/test/utils/system_test_utils.hpp b/husarion_ugv_hardware_interfaces/test/utils/system_test_utils.hpp
similarity index 75%
rename from panther_hardware_interfaces/test/utils/system_test_utils.hpp
rename to husarion_ugv_hardware_interfaces/test/utils/system_test_utils.hpp
index be79f931..7797002e 100644
--- a/panther_hardware_interfaces/test/utils/system_test_utils.hpp
+++ b/husarion_ugv_hardware_interfaces/test/utils/system_test_utils.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_TEST_SYSTEM_TEST_UTILS_HPP_
-#define PANTHER_HARDWARE_INTERFACES_TEST_SYSTEM_TEST_UTILS_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_TEST_SYSTEM_TEST_UTILS_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_TEST_SYSTEM_TEST_UTILS_HPP_
 
 #include <cstdint>
 #include <future>
@@ -22,16 +22,16 @@
 
 #include <gmock/gmock.h>
 
-#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp"
-#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp"
-#include "panther_hardware_interfaces/panther_system/system_e_stop.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp"
+#include "husarion_ugv_hardware_interfaces/robot_system/system_e_stop.hpp"
 
 #include "utils/mock_driver.hpp"
 
-namespace panther_hardware_interfaces_test
+namespace husarion_ugv_hardware_interfaces_test
 {
 
-class MockRobotDriver : public panther_hardware_interfaces::RobotDriverInterface
+class MockRobotDriver : public husarion_ugv_hardware_interfaces::RobotDriverInterface
 {
 public:
   MOCK_METHOD(void, Initialize, (), (override));
@@ -41,7 +41,8 @@ class MockRobotDriver : public panther_hardware_interfaces::RobotDriverInterface
   MOCK_METHOD(void, UpdateMotorsState, (), (override));
   MOCK_METHOD(void, UpdateDriversState, (), (override));
   MOCK_METHOD(
-    const panther_hardware_interfaces::DriverData &, GetData, (const std::string &), (override));
+    const husarion_ugv_hardware_interfaces::DriverData &, GetData, (const std::string &),
+    (override));
   MOCK_METHOD(void, SendSpeedCommands, (const std::vector<float> &), (override));
   MOCK_METHOD(void, TurnOnEStop, (), (override));
   MOCK_METHOD(void, TurnOffEStop, (), (override));
@@ -51,26 +52,26 @@ class MockRobotDriver : public panther_hardware_interfaces::RobotDriverInterface
   using NiceMock = testing::NiceMock<MockRobotDriver>;
 };
 
-class MockGPIODriver : public panther_hardware_interfaces::GPIODriverInterface
+class MockGPIODriver : public husarion_ugv_hardware_interfaces::GPIODriverInterface
 {
 public:
   MOCK_METHOD(void, GPIOMonitorEnable, (const bool, const unsigned), (override));
   MOCK_METHOD(
     void, ConfigureEdgeEventCallback,
-    (const std::function<void(const panther_hardware_interfaces::GPIOInfo &)> &), (override));
+    (const std::function<void(const husarion_ugv_hardware_interfaces::GPIOInfo &)> &), (override));
   MOCK_METHOD(
     void, ChangePinDirection,
-    (const panther_hardware_interfaces::GPIOPin, const gpiod::line::direction), (override));
+    (const husarion_ugv_hardware_interfaces::GPIOPin, const gpiod::line::direction), (override));
   MOCK_METHOD(
-    bool, IsPinAvailable, (const panther_hardware_interfaces::GPIOPin), (const, override));
-  MOCK_METHOD(bool, IsPinActive, (const panther_hardware_interfaces::GPIOPin), (override));
+    bool, IsPinAvailable, (const husarion_ugv_hardware_interfaces::GPIOPin), (const, override));
+  MOCK_METHOD(bool, IsPinActive, (const husarion_ugv_hardware_interfaces::GPIOPin), (override));
   MOCK_METHOD(
-    bool, SetPinValue, (const panther_hardware_interfaces::GPIOPin, const bool), (override));
+    bool, SetPinValue, (const husarion_ugv_hardware_interfaces::GPIOPin, const bool), (override));
 
   using NiceMock = testing::NiceMock<MockGPIODriver>;
 };
 
-class MockGPIOController : public panther_hardware_interfaces::GPIOControllerInterface
+class MockGPIOController : public husarion_ugv_hardware_interfaces::GPIOControllerInterface
 {
 public:
   MockGPIOController() : GPIOControllerInterface()
@@ -88,13 +89,13 @@ class MockGPIOController : public panther_hardware_interfaces::GPIOControllerInt
   MOCK_METHOD(bool, ChargerEnable, (const bool), (override));
   MOCK_METHOD(bool, LEDControlEnable, (const bool), (override));
   MOCK_METHOD(
-    (std::unordered_map<panther_hardware_interfaces::GPIOPin, bool>), QueryControlInterfaceIOStates,
-    (), (const, override));
+    (std::unordered_map<husarion_ugv_hardware_interfaces::GPIOPin, bool>),
+    QueryControlInterfaceIOStates, (), (const, override));
 
   using NiceMock = testing::NiceMock<MockGPIOController>;
 };
 
-class MockEStop : public panther_hardware_interfaces::EStopInterface
+class MockEStop : public husarion_ugv_hardware_interfaces::EStopInterface
 {
 public:
   MockEStop() : EStopInterface() {}
@@ -169,6 +170,6 @@ hardware_interface::HardwareInfo GenerateDefaultHardwareInfo()
   return hardware_info;
 }
 
-}  // namespace panther_hardware_interfaces_test
+}  // namespace husarion_ugv_hardware_interfaces_test
 
-#endif  // PANTHER_HARDWARE_INTERFACES_TEST_SYSTEM_TEST_UTILS_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_TEST_SYSTEM_TEST_UTILS_HPP_
diff --git a/panther_hardware_interfaces/test/utils/test_constants.hpp b/husarion_ugv_hardware_interfaces/test/utils/test_constants.hpp
similarity index 78%
rename from panther_hardware_interfaces/test/utils/test_constants.hpp
rename to husarion_ugv_hardware_interfaces/test/utils/test_constants.hpp
index 5b65c55e..af66e9c3 100644
--- a/panther_hardware_interfaces/test/utils/test_constants.hpp
+++ b/husarion_ugv_hardware_interfaces/test/utils/test_constants.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_HARDWARE_INTERFACES_TEST_UTILS_TEST_CONSTANTS_HPP_
-#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_TEST_CONSTANTS_HPP_
+#ifndef HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_TEST_CONSTANTS_HPP_
+#define HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_TEST_CONSTANTS_HPP_
 
 #include <chrono>
 #include <cmath>
@@ -21,13 +21,13 @@
 #include <string>
 #include <vector>
 
-#include <panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp>
-#include <panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp>
+#include <husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp>
+#include <husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp>
 
-namespace panther_hardware_interfaces_test
+namespace husarion_ugv_hardware_interfaces_test
 {
 
-const panther_hardware_interfaces::CANopenSettings kCANopenSettings{
+const husarion_ugv_hardware_interfaces::CANopenSettings kCANopenSettings{
   "robot_can",
   3,
   {{"default", 1}, {"front", 1}, {"rear", 2}},
@@ -36,7 +36,7 @@ const panther_hardware_interfaces::CANopenSettings kCANopenSettings{
   std::chrono::milliseconds(100),
 };
 
-const panther_hardware_interfaces::DrivetrainSettings kDrivetrainSettings{
+const husarion_ugv_hardware_interfaces::DrivetrainSettings kDrivetrainSettings{
   0.11, 30.08, 0.75, 1600.0, 3600.0};
 
 constexpr float kRadPerSecToRbtqCmd = 30.08 * (1.0 / (2.0 * M_PI)) * 60.0 * (1000.0 / 3600.0);
@@ -63,7 +63,7 @@ const std::string kJointInterfaces =
 )";
 
 const std::string kPluginName =
-  R"(<plugin>panther_hardware_interfaces/PantherSystem</plugin>
+  R"(<plugin>husarion_ugv_hardware_interfaces/PantherSystem</plugin>
 )";
 
 const std::map<std::string, std::string> kDefaultParamMap = {
@@ -92,6 +92,6 @@ const std::vector<std::string> kDefaultJoints = {
 
 const std::string kRobotDriverStateTopic = "/hardware_controller/robot_driver_state";
 
-}  // namespace panther_hardware_interfaces_test
+}  // namespace husarion_ugv_hardware_interfaces_test
 
-#endif  // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_TEST_CONSTANTS_HPP_
+#endif  // HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_TEST_CONSTANTS_HPP_
diff --git a/panther_lights/.docs/AUTONOMOUS_ACTION.webp b/husarion_ugv_lights/.docs/AUTONOMOUS_ACTION.webp
similarity index 100%
rename from panther_lights/.docs/AUTONOMOUS_ACTION.webp
rename to husarion_ugv_lights/.docs/AUTONOMOUS_ACTION.webp
diff --git a/panther_lights/.docs/BATTERY_STATE.webp b/husarion_ugv_lights/.docs/BATTERY_STATE.webp
similarity index 100%
rename from panther_lights/.docs/BATTERY_STATE.webp
rename to husarion_ugv_lights/.docs/BATTERY_STATE.webp
diff --git a/panther_lights/.docs/CHARGING_BATTERY.webp b/husarion_ugv_lights/.docs/CHARGING_BATTERY.webp
similarity index 100%
rename from panther_lights/.docs/CHARGING_BATTERY.webp
rename to husarion_ugv_lights/.docs/CHARGING_BATTERY.webp
diff --git a/panther_lights/.docs/CRITICAL_BATTERY.webp b/husarion_ugv_lights/.docs/CRITICAL_BATTERY.webp
similarity index 100%
rename from panther_lights/.docs/CRITICAL_BATTERY.webp
rename to husarion_ugv_lights/.docs/CRITICAL_BATTERY.webp
diff --git a/panther_lights/.docs/ERROR.webp b/husarion_ugv_lights/.docs/ERROR.webp
similarity index 100%
rename from panther_lights/.docs/ERROR.webp
rename to husarion_ugv_lights/.docs/ERROR.webp
diff --git a/panther_lights/.docs/E_STOP.webp b/husarion_ugv_lights/.docs/E_STOP.webp
similarity index 100%
rename from panther_lights/.docs/E_STOP.webp
rename to husarion_ugv_lights/.docs/E_STOP.webp
diff --git a/panther_lights/.docs/GOAL_ACHIEVED.webp b/husarion_ugv_lights/.docs/GOAL_ACHIEVED.webp
similarity index 100%
rename from panther_lights/.docs/GOAL_ACHIEVED.webp
rename to husarion_ugv_lights/.docs/GOAL_ACHIEVED.webp
diff --git a/panther_lights/.docs/LOW_BATTERY.webp b/husarion_ugv_lights/.docs/LOW_BATTERY.webp
similarity index 100%
rename from panther_lights/.docs/LOW_BATTERY.webp
rename to husarion_ugv_lights/.docs/LOW_BATTERY.webp
diff --git a/panther_lights/.docs/MANUAL_ACTION.webp b/husarion_ugv_lights/.docs/MANUAL_ACTION.webp
similarity index 100%
rename from panther_lights/.docs/MANUAL_ACTION.webp
rename to husarion_ugv_lights/.docs/MANUAL_ACTION.webp
diff --git a/panther_lights/.docs/READY.webp b/husarion_ugv_lights/.docs/READY.webp
similarity index 100%
rename from panther_lights/.docs/READY.webp
rename to husarion_ugv_lights/.docs/READY.webp
diff --git a/panther_lights/CHANGELOG.rst b/husarion_ugv_lights/CHANGELOG.rst
similarity index 100%
rename from panther_lights/CHANGELOG.rst
rename to husarion_ugv_lights/CHANGELOG.rst
diff --git a/panther_lights/CMakeLists.txt b/husarion_ugv_lights/CMakeLists.txt
similarity index 86%
rename from panther_lights/CMakeLists.txt
rename to husarion_ugv_lights/CMakeLists.txt
index 482023e3..3fc9de63 100644
--- a/panther_lights/CMakeLists.txt
+++ b/husarion_ugv_lights/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10.2)
-project(panther_lights)
+project(husarion_ugv_lights)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
@@ -10,7 +10,7 @@ set(PACKAGE_DEPENDENCIES
     diagnostic_updater
     image_transport
     panther_msgs
-    panther_utils
+    husarion_ugv_utils
     pluginlib
     rclcpp
     rclcpp_components
@@ -26,11 +26,10 @@ include_directories(include)
 
 pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml)
 
-add_library(
-  panther_animation_plugins SHARED src/animation/image_animation.cpp
-                                   src/animation/charging_animation.cpp)
-ament_target_dependencies(panther_animation_plugins panther_utils pluginlib)
-target_link_libraries(panther_animation_plugins png yaml-cpp)
+add_library(animation_plugins SHARED src/animation/image_animation.cpp
+                                     src/animation/charging_animation.cpp)
+ament_target_dependencies(animation_plugins husarion_ugv_utils pluginlib)
+target_link_libraries(animation_plugins png yaml-cpp)
 
 add_library(lights_driver_node_component SHARED src/lights_driver_node.cpp
                                                 src/apa102.cpp)
@@ -52,7 +51,7 @@ add_library(
 ament_target_dependencies(
   lights_controller_node_component
   panther_msgs
-  panther_utils
+  husarion_ugv_utils
   pluginlib
   rclcpp
   rclcpp_components
@@ -60,12 +59,12 @@ ament_target_dependencies(
 target_link_libraries(lights_controller_node_component yaml-cpp)
 
 rclcpp_components_register_node(
-  lights_driver_node_component PLUGIN "panther_lights::LightsDriverNode"
+  lights_driver_node_component PLUGIN "husarion_ugv_lights::LightsDriverNode"
   EXECUTABLE lights_driver_node)
 
 rclcpp_components_register_node(
   lights_controller_node_component PLUGIN
-  "panther_lights::LightsControllerNode" EXECUTABLE lights_controller_node)
+  "husarion_ugv_lights::LightsControllerNode" EXECUTABLE lights_controller_node)
 
 ament_export_targets(export_lights_driver_node_component)
 install(
@@ -83,7 +82,7 @@ install(
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin)
 
-install(TARGETS panther_animation_plugins LIBRARY DESTINATION lib)
+install(TARGETS animation_plugins LIBRARY DESTINATION lib)
 
 install(DIRECTORY animations config launch test
         DESTINATION share/${PROJECT_NAME})
@@ -100,7 +99,7 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_animation
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
            $<INSTALL_INTERFACE:include>)
-  ament_target_dependencies(${PROJECT_NAME}_test_animation panther_utils)
+  ament_target_dependencies(${PROJECT_NAME}_test_animation husarion_ugv_utils)
   target_link_libraries(${PROJECT_NAME}_test_animation yaml-cpp)
 
   ament_add_gtest(
@@ -112,7 +111,7 @@ if(BUILD_TESTING)
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_charging_animation
-                            ament_index_cpp panther_utils pluginlib)
+                            ament_index_cpp husarion_ugv_utils pluginlib)
   target_link_libraries(${PROJECT_NAME}_test_charging_animation yaml-cpp)
 
   ament_add_gtest(
@@ -124,7 +123,7 @@ if(BUILD_TESTING)
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_image_animation
-                            ament_index_cpp panther_utils pluginlib)
+                            ament_index_cpp husarion_ugv_utils pluginlib)
   target_link_libraries(${PROJECT_NAME}_test_image_animation png yaml-cpp)
 
   ament_add_gtest(
@@ -144,7 +143,7 @@ if(BUILD_TESTING)
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_led_segment pluginlib
-                            panther_utils)
+                            husarion_ugv_utils)
   target_link_libraries(${PROJECT_NAME}_test_led_segment yaml-cpp)
 
   ament_add_gtest(
@@ -158,7 +157,7 @@ if(BUILD_TESTING)
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_segment_converter pluginlib
-                            panther_utils)
+                            husarion_ugv_utils)
   target_link_libraries(${PROJECT_NAME}_test_segment_converter yaml-cpp)
 
   ament_add_gtest(
@@ -172,7 +171,7 @@ if(BUILD_TESTING)
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_led_animation pluginlib
-                            panther_utils rclcpp)
+                            husarion_ugv_utils rclcpp)
   target_link_libraries(${PROJECT_NAME}_test_led_animation yaml-cpp)
 
   ament_add_gtest(
@@ -186,7 +185,7 @@ if(BUILD_TESTING)
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_led_animations_queue pluginlib
-                            panther_utils rclcpp)
+                            husarion_ugv_utils rclcpp)
   target_link_libraries(${PROJECT_NAME}_test_led_animations_queue yaml-cpp)
 
   ament_add_gmock(${PROJECT_NAME}_test_apa102 test/unit/test_apa102.cpp
@@ -205,7 +204,7 @@ if(BUILD_TESTING)
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_lights_driver_node
-                            panther_utils)
+                            husarion_ugv_utils)
   target_link_libraries(${PROJECT_NAME}_test_lights_driver_node
                         lights_driver_node_component)
 
@@ -222,19 +221,19 @@ if(BUILD_TESTING)
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_lights_controller_node
-                            panther_utils)
+                            husarion_ugv_utils)
   target_link_libraries(${PROJECT_NAME}_test_lights_controller_node
                         lights_controller_node_component yaml-cpp)
 
   # Integration tests
   option(TEST_INTEGRATION "Run integration tests" OFF)
   if(TEST_INTEGRATION)
-    add_ros_test(test/integration/panther_lights.test.py)
+    add_ros_test(test/integration/husarion_ugv_lights.test.py)
   endif()
 
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(panther_animation_plugins)
+ament_export_libraries(animation_plugins)
 
 ament_package()
diff --git a/panther_lights/CONFIGURATION.md b/husarion_ugv_lights/CONFIGURATION.md
similarity index 88%
rename from panther_lights/CONFIGURATION.md
rename to husarion_ugv_lights/CONFIGURATION.md
index 2c6d6421..986aba94 100644
--- a/panther_lights/CONFIGURATION.md
+++ b/husarion_ugv_lights/CONFIGURATION.md
@@ -1,4 +1,4 @@
-# panther_light
+# husarion_ugv_lights
 
 ## LED Animations
 
@@ -44,7 +44,7 @@ The `segments_map` section allows creating named groups of segments on which ani
 The `led_animations` section contains a list with definitions for various animations that can be displayed on the LED segments. Supported keys are:
 
 - `animations` [*list*, default: **None**]: definition of animation for each Bumper Lights. Supported keys are:
-  - `type` [*string*, default **None**]: Specifies the type of animation. Default animation types are: `panther_lights::ImageAnimation`, `panther_lights::ChargingAnimation`.
+  - `type` [*string*, default **None**]: Specifies the type of animation. Default animation types are: `husarion_ugv_lights::ImageAnimation`, `husarion_ugv_lights::ChargingAnimation`.
   - `segments` [*string*, default **None**]: Indicates which segment mapping this particular animation applies to (e.g., all, front, rear).
   - `animation` [*yaml*, default: **None**]: An animation to be displayed on segments. The keys for the configuration of different animation types are explained in detail under the [**Animation Types**](#animation-types) section.
 - `id` [*int*, default: **None**]: unique ID of an animation.
@@ -70,14 +70,14 @@ Basic animation definition. Keys are inherited from the basic **Animation** clas
 
 #### ImageAnimation
 
-Animation of type `panther_lights::ImageAnimation`, returns frames to display based on a supplied image. Extends `Animation` with keys:
+Animation of type `husarion_ugv_lights::ImageAnimation`, returns frames to display based on a supplied image. Extends `Animation` with keys:
 
 - `color` [*int*, default: **None**]: The image is turned into grayscale, and then the color is applied with brightness from the gray image. Values have to be in HEX format. This parameter is not required.
 - `image` [*string*, default: **None**]: path to an image file. Only global paths are valid. Allows using `$(find ros_package)` syntax.
 
 #### ChargingAnimation
 
-Animation of type `panther_lights::ChargingAnimation`, returns frame to display based on `param` value representing Battery percentage. Displays a solid color with a duty cycle proportional to the Battery percentage. The color is changing from red (Battery discharged) to green (Battery fully charged).
+Animation of type `husarion_ugv_lights::ChargingAnimation`, returns frame to display based on `param` value representing Battery percentage. Displays a solid color with a duty cycle proportional to the Battery percentage. The color is changing from red (Battery discharged) to green (Battery fully charged).
 
 ### Defining Animations
 
@@ -93,10 +93,10 @@ user_animations:
     name: ANIMATION_1
     priority: 2
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/strip01_red.png
+          image: $(find husarion_ugv_lights)/animations/strip01_red.png
           duration: 2
           repeat: 2
           color: 0xffff00
@@ -106,7 +106,7 @@ user_animations:
     name: ANIMATION_2
     priority: 3
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
           image: /animations/custom_image.png
@@ -118,7 +118,7 @@ user_animations:
     name: ANIMATION_3
     priority: 3
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
           image: $(find custom_pkg)/animations/custom_image.png
@@ -130,13 +130,13 @@ user_animations:
     name: ANIMATION_4
     priority: 3
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: front
         animation:
           image: $(find custom_pkg)/animations/front_custom_image.png
           duration: 2
           repeat: 2
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: rear
         animation:
           image: $(find custom_pkg)/animations/rear_custom_image.png
@@ -152,7 +152,7 @@ user_animations:
 Remember to modify launch command to use user animations:
 
 ``` bash
-ros2 launch panther_bringup bringup.launch user_animations_file:=/my_awesome_user_animations.yaml
+ros2 launch husarion_ugv_bringup bringup.launch user_animations_file:=/my_awesome_user_animations.yaml
 ```
 
 Test new animations:
diff --git a/panther_lights/LIGHTS_API.md b/husarion_ugv_lights/LIGHTS_API.md
similarity index 92%
rename from panther_lights/LIGHTS_API.md
rename to husarion_ugv_lights/LIGHTS_API.md
index 4def8a0e..cef272ad 100644
--- a/panther_lights/LIGHTS_API.md
+++ b/husarion_ugv_lights/LIGHTS_API.md
@@ -35,7 +35,7 @@ Getters:
 
 ## Defining a New Animation Type
 
-It is possible to define your own animation type with expected, new behavior. All animation definitions inherit from the basic `Animation` class. Animations are loaded using `pluginlib` and can be defined from any point in your project. All you need is to import `Animation` class from `panther_lights` package and export it as a pluginlib plugin. For more information about creating and managing pluginlib see: [Creating and using plugins (C++)](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Pluginlib.html).
+It is possible to define your own animation type with expected, new behavior. All animation definitions inherit from the basic `Animation` class. Animations are loaded using `pluginlib` and can be defined from any point in your project. All you need is to import `Animation` class from `husarion_ugv_lights` package and export it as a pluginlib plugin. For more information about creating and managing pluginlib see: [Creating and using plugins (C++)](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Pluginlib.html).
 
 Frames are processed in ticks with a frequency of `controller_frequency`. It is required to overwrite the `UpdateFrame` method which must return a list representing the frame for a given tick. The advised way is to use the `GetAnimationIteration` (current animation tick) as a time base for animation frames. The length of the frame has to match `num_led`. Each element of the frame represents a color for a single LED in the Bumper Lights. Colors are described as a list of integers with respective **R**, **G**, and **B** color values and **A** alpha channel. Additional parameters (e.g. image path) can be passed within `animation_description` and processed inside `Initialize` method. See the example below or other animation definitions.
 
@@ -49,7 +49,7 @@ Create a New Animation Type:
 
 #include "yaml-cpp/yaml.h"
 
-#include "panther_lights/animation/animation.hpp"
+#include "husarion_ugv_lights/animation/animation.hpp"
 
 class MyCoolAnimation : public Animation
 {
diff --git a/panther_lights/README.md b/husarion_ugv_lights/README.md
similarity index 81%
rename from panther_lights/README.md
rename to husarion_ugv_lights/README.md
index 548b0403..50a8c4e6 100644
--- a/panther_lights/README.md
+++ b/husarion_ugv_lights/README.md
@@ -1,12 +1,12 @@
-# panther_lights
+# husarion_ugv_lights
 
-Package used to control the Husarion Panther Bumper Lights.
+Package used to control the Husarion UGV robot's lights.
 
 ## Launch files
 
 This package contains:
 
-- `lights.launch.py`: Responsible for launching the nodes required to control the Panther Bumper Lights.
+- `lights.launch.py`: Responsible for launching the nodes required to control the robot's lights.
 
 ## Configuration Files
 
@@ -17,7 +17,7 @@ This package contains:
 
 ### LightsControllerNode
 
-This node is of type rclcpp_components is responsible for processing animations and publishing frames to be displayed on the Husarion Panther robot Bumper Lights.
+This node is of type rclcpp_components is responsible for processing animations and publishing frames to `light_driver` node.
 
 #### Publishers
 
@@ -30,13 +30,13 @@ This node is of type rclcpp_components is responsible for processing animations
 
 #### Parameters
 
-- `~animations_config_path` [*string*, default: **$(find panther_lights)/panther_lights/config/{robot_model}_animations.yaml**]: Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations.
+- `~animations_config_path` [*string*, default: **$(find husarion_ugv_lights)/husarion_ugv_lights/config/{robot_model}_animations.yaml**]: Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations.
 - `~controller_frequency` [*float*, default: **50.0**]: Frequency [Hz] at which the lights controller node will process animations.
 - `~user_led_animations_path` [*string*, default: **None**]: Path to a YAML file with a description of the user defined animations.
 
 ### LightsDriverNode
 
-This node is of type rclcpp_components is responsible for displaying frames on the Husarion Panther robot's Bumper Lights.
+This node is of type rclcpp_components is responsible for displaying frames on the robot's lights.
 
 #### Publishers
 
diff --git a/panther_lights/animations/lynx/battery_critical.png b/husarion_ugv_lights/animations/lynx/battery_critical.png
similarity index 100%
rename from panther_lights/animations/lynx/battery_critical.png
rename to husarion_ugv_lights/animations/lynx/battery_critical.png
diff --git a/panther_lights/animations/lynx/battery_low.png b/husarion_ugv_lights/animations/lynx/battery_low.png
similarity index 100%
rename from panther_lights/animations/lynx/battery_low.png
rename to husarion_ugv_lights/animations/lynx/battery_low.png
diff --git a/panther_lights/animations/lynx/strip01_purple.png b/husarion_ugv_lights/animations/lynx/strip01_purple.png
similarity index 100%
rename from panther_lights/animations/lynx/strip01_purple.png
rename to husarion_ugv_lights/animations/lynx/strip01_purple.png
diff --git a/panther_lights/animations/lynx/strip01_red.png b/husarion_ugv_lights/animations/lynx/strip01_red.png
similarity index 100%
rename from panther_lights/animations/lynx/strip01_red.png
rename to husarion_ugv_lights/animations/lynx/strip01_red.png
diff --git a/panther_lights/animations/lynx/triangle01_blue.png b/husarion_ugv_lights/animations/lynx/triangle01_blue.png
similarity index 100%
rename from panther_lights/animations/lynx/triangle01_blue.png
rename to husarion_ugv_lights/animations/lynx/triangle01_blue.png
diff --git a/panther_lights/animations/lynx/triangle01_cyan.png b/husarion_ugv_lights/animations/lynx/triangle01_cyan.png
similarity index 100%
rename from panther_lights/animations/lynx/triangle01_cyan.png
rename to husarion_ugv_lights/animations/lynx/triangle01_cyan.png
diff --git a/panther_lights/animations/lynx/triangle01_green.png b/husarion_ugv_lights/animations/lynx/triangle01_green.png
similarity index 100%
rename from panther_lights/animations/lynx/triangle01_green.png
rename to husarion_ugv_lights/animations/lynx/triangle01_green.png
diff --git a/panther_lights/animations/lynx/triangle01_orange.png b/husarion_ugv_lights/animations/lynx/triangle01_orange.png
similarity index 100%
rename from panther_lights/animations/lynx/triangle01_orange.png
rename to husarion_ugv_lights/animations/lynx/triangle01_orange.png
diff --git a/panther_lights/animations/lynx/triangle01_purple.png b/husarion_ugv_lights/animations/lynx/triangle01_purple.png
similarity index 100%
rename from panther_lights/animations/lynx/triangle01_purple.png
rename to husarion_ugv_lights/animations/lynx/triangle01_purple.png
diff --git a/panther_lights/animations/lynx/triangle01_red.png b/husarion_ugv_lights/animations/lynx/triangle01_red.png
similarity index 100%
rename from panther_lights/animations/lynx/triangle01_red.png
rename to husarion_ugv_lights/animations/lynx/triangle01_red.png
diff --git a/panther_lights/animations/lynx/triangle01_yellow.png b/husarion_ugv_lights/animations/lynx/triangle01_yellow.png
similarity index 100%
rename from panther_lights/animations/lynx/triangle01_yellow.png
rename to husarion_ugv_lights/animations/lynx/triangle01_yellow.png
diff --git a/panther_lights/animations/panther/battery_critical.png b/husarion_ugv_lights/animations/panther/battery_critical.png
similarity index 100%
rename from panther_lights/animations/panther/battery_critical.png
rename to husarion_ugv_lights/animations/panther/battery_critical.png
diff --git a/panther_lights/animations/panther/battery_low.png b/husarion_ugv_lights/animations/panther/battery_low.png
similarity index 100%
rename from panther_lights/animations/panther/battery_low.png
rename to husarion_ugv_lights/animations/panther/battery_low.png
diff --git a/panther_lights/animations/panther/strip01_purple.png b/husarion_ugv_lights/animations/panther/strip01_purple.png
similarity index 100%
rename from panther_lights/animations/panther/strip01_purple.png
rename to husarion_ugv_lights/animations/panther/strip01_purple.png
diff --git a/panther_lights/animations/panther/strip01_red.png b/husarion_ugv_lights/animations/panther/strip01_red.png
similarity index 100%
rename from panther_lights/animations/panther/strip01_red.png
rename to husarion_ugv_lights/animations/panther/strip01_red.png
diff --git a/panther_lights/animations/panther/triangle01_blue.png b/husarion_ugv_lights/animations/panther/triangle01_blue.png
similarity index 100%
rename from panther_lights/animations/panther/triangle01_blue.png
rename to husarion_ugv_lights/animations/panther/triangle01_blue.png
diff --git a/panther_lights/animations/panther/triangle01_cyan.png b/husarion_ugv_lights/animations/panther/triangle01_cyan.png
similarity index 100%
rename from panther_lights/animations/panther/triangle01_cyan.png
rename to husarion_ugv_lights/animations/panther/triangle01_cyan.png
diff --git a/panther_lights/animations/panther/triangle01_green.png b/husarion_ugv_lights/animations/panther/triangle01_green.png
similarity index 100%
rename from panther_lights/animations/panther/triangle01_green.png
rename to husarion_ugv_lights/animations/panther/triangle01_green.png
diff --git a/panther_lights/animations/panther/triangle01_orange.png b/husarion_ugv_lights/animations/panther/triangle01_orange.png
similarity index 100%
rename from panther_lights/animations/panther/triangle01_orange.png
rename to husarion_ugv_lights/animations/panther/triangle01_orange.png
diff --git a/panther_lights/animations/panther/triangle01_purple.png b/husarion_ugv_lights/animations/panther/triangle01_purple.png
similarity index 100%
rename from panther_lights/animations/panther/triangle01_purple.png
rename to husarion_ugv_lights/animations/panther/triangle01_purple.png
diff --git a/panther_lights/animations/panther/triangle01_red.png b/husarion_ugv_lights/animations/panther/triangle01_red.png
similarity index 100%
rename from panther_lights/animations/panther/triangle01_red.png
rename to husarion_ugv_lights/animations/panther/triangle01_red.png
diff --git a/panther_lights/animations/panther/triangle01_yellow.png b/husarion_ugv_lights/animations/panther/triangle01_yellow.png
similarity index 100%
rename from panther_lights/animations/panther/triangle01_yellow.png
rename to husarion_ugv_lights/animations/panther/triangle01_yellow.png
diff --git a/panther_lights/config/lynx_animations.yaml b/husarion_ugv_lights/config/lynx_animations.yaml
similarity index 62%
rename from panther_lights/config/lynx_animations.yaml
rename to husarion_ugv_lights/config/lynx_animations.yaml
index 87252652..c6b0c477 100644
--- a/panther_lights/config/lynx_animations.yaml
+++ b/husarion_ugv_lights/config/lynx_animations.yaml
@@ -32,30 +32,30 @@ led_animations:
     name: E_STOP
     priority: 3
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/lynx/triangle01_red.png
+          image: $(find husarion_ugv_lights)/animations/lynx/triangle01_red.png
           duration: 2
 
   - id: 1
     name: READY
     priority: 3
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/lynx/triangle01_green.png
+          image: $(find husarion_ugv_lights)/animations/lynx/triangle01_green.png
           duration: 2
 
   - id: 2
     name: ERROR
     priority: 1
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/lynx/strip01_red.png
+          image: $(find husarion_ugv_lights)/animations/lynx/strip01_red.png
           duration: 2
           repeat: 2
 
@@ -63,30 +63,30 @@ led_animations:
     name: MANUAL_ACTION
     priority: 3
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/lynx/triangle01_blue.png
+          image: $(find husarion_ugv_lights)/animations/lynx/triangle01_blue.png
           duration: 3
 
   - id: 4
     name: AUTONOMOUS_ACTION
     priority: 3
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/lynx/triangle01_orange.png
+          image: $(find husarion_ugv_lights)/animations/lynx/triangle01_orange.png
           duration: 3
 
   - id: 5
     name: GOAL_ACHIEVED
     priority: 2
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/lynx/strip01_purple.png
+          image: $(find husarion_ugv_lights)/animations/lynx/strip01_purple.png
           duration: 3
           repeat: 2
 
@@ -94,10 +94,10 @@ led_animations:
     name: LOW_BATTERY
     priority: 2
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/lynx/battery_low.png
+          image: $(find husarion_ugv_lights)/animations/lynx/battery_low.png
           duration: 2
           repeat: 2
 
@@ -105,10 +105,10 @@ led_animations:
     name: CRITICAL_BATTERY
     priority: 2
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/lynx/battery_critical.png
+          image: $(find husarion_ugv_lights)/animations/lynx/battery_critical.png
           duration: 2
           repeat: 2
 
@@ -119,7 +119,7 @@ led_animations:
     name: CHARGING_BATTERY
     priority: 3
     animations:
-      - type: panther_lights::ChargingAnimation
+      - type: husarion_ugv_lights::ChargingAnimation
         segments: all
         animation:
           duration: 3
diff --git a/panther_lights/config/lynx_driver.yaml b/husarion_ugv_lights/config/lynx_driver.yaml
similarity index 100%
rename from panther_lights/config/lynx_driver.yaml
rename to husarion_ugv_lights/config/lynx_driver.yaml
diff --git a/panther_lights/config/panther_animations.yaml b/husarion_ugv_lights/config/panther_animations.yaml
similarity index 60%
rename from panther_lights/config/panther_animations.yaml
rename to husarion_ugv_lights/config/panther_animations.yaml
index 304640a9..fc574749 100644
--- a/panther_lights/config/panther_animations.yaml
+++ b/husarion_ugv_lights/config/panther_animations.yaml
@@ -22,30 +22,30 @@ led_animations:
     name: E_STOP
     priority: 3
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/panther/triangle01_red.png
+          image: $(find husarion_ugv_lights)/animations/panther/triangle01_red.png
           duration: 2
 
   - id: 1
     name: READY
     priority: 3
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/panther/triangle01_green.png
+          image: $(find husarion_ugv_lights)/animations/panther/triangle01_green.png
           duration: 2
 
   - id: 2
     name: ERROR
     priority: 1
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/panther/strip01_red.png
+          image: $(find husarion_ugv_lights)/animations/panther/strip01_red.png
           duration: 2
           repeat: 2
 
@@ -53,30 +53,30 @@ led_animations:
     name: MANUAL_ACTION
     priority: 3
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/panther/triangle01_blue.png
+          image: $(find husarion_ugv_lights)/animations/panther/triangle01_blue.png
           duration: 3
 
   - id: 4
     name: AUTONOMOUS_ACTION
     priority: 3
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/panther/triangle01_orange.png
+          image: $(find husarion_ugv_lights)/animations/panther/triangle01_orange.png
           duration: 3
 
   - id: 5
     name: GOAL_ACHIEVED
     priority: 2
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/panther/strip01_purple.png
+          image: $(find husarion_ugv_lights)/animations/panther/strip01_purple.png
           duration: 3
           repeat: 2
 
@@ -84,10 +84,10 @@ led_animations:
     name: LOW_BATTERY
     priority: 2
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/panther/battery_low.png
+          image: $(find husarion_ugv_lights)/animations/panther/battery_low.png
           duration: 2
           repeat: 2
 
@@ -95,10 +95,10 @@ led_animations:
     name: CRITICAL_BATTERY
     priority: 2
     animations:
-      - type: panther_lights::ImageAnimation
+      - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find panther_lights)/animations/panther/battery_critical.png
+          image: $(find husarion_ugv_lights)/animations/panther/battery_critical.png
           duration: 2
           repeat: 2
 
@@ -109,7 +109,7 @@ led_animations:
     name: CHARGING_BATTERY
     priority: 3
     animations:
-      - type: panther_lights::ChargingAnimation
+      - type: husarion_ugv_lights::ChargingAnimation
         segments: all
         animation:
           duration: 3
diff --git a/panther_lights/config/panther_driver.yaml b/husarion_ugv_lights/config/panther_driver.yaml
similarity index 100%
rename from panther_lights/config/panther_driver.yaml
rename to husarion_ugv_lights/config/panther_driver.yaml
diff --git a/panther_lights/include/panther_lights/animation/animation.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/animation/animation.hpp
similarity index 91%
rename from panther_lights/include/panther_lights/animation/animation.hpp
rename to husarion_ugv_lights/include/husarion_ugv_lights/animation/animation.hpp
index 9c237b1d..98c9bf1d 100644
--- a/panther_lights/include/panther_lights/animation/animation.hpp
+++ b/husarion_ugv_lights/include/husarion_ugv_lights/animation/animation.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_LIGHTS_ANIMATION_ANIMATION_HPP_
-#define PANTHER_LIGHTS_ANIMATION_ANIMATION_HPP_
+#ifndef HUSARION_UGV_LIGHTS_ANIMATION_ANIMATION_HPP_
+#define HUSARION_UGV_LIGHTS_ANIMATION_ANIMATION_HPP_
 
 #include <algorithm>
 #include <cmath>
@@ -25,9 +25,9 @@
 
 #include "yaml-cpp/yaml.h"
 
-#include "panther_utils/yaml_utils.hpp"
+#include "husarion_ugv_utils/yaml_utils.hpp"
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 class Animation
@@ -53,12 +53,12 @@ class Animation
     num_led_ = num_led;
     frame_ = std::vector<std::uint8_t>(num_led_ * kRGBAColorLen, 0);
 
-    auto duration = panther_utils::GetYAMLKeyValue<float>(animation_description, "duration");
+    auto duration = husarion_ugv_utils::GetYAMLKeyValue<float>(animation_description, "duration");
     if ((duration - std::numeric_limits<float>::epsilon()) <= 0.0) {
       throw std::out_of_range("Duration has to be positive");
     }
 
-    loops_ = panther_utils::GetYAMLKeyValue<std::size_t>(animation_description, "repeat", 1);
+    loops_ = husarion_ugv_utils::GetYAMLKeyValue<std::size_t>(animation_description, "repeat", 1);
 
     if (duration * loops_ > 10.0) {
       throw std::runtime_error("Animation display duration (duration * repeat) exceeds 10 seconds");
@@ -187,6 +187,6 @@ class Animation
   std::vector<std::uint8_t> frame_;
 };
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
 
-#endif  // PANTHER_LIGHTS_ANIMATION_ANIMATION_HPP_
+#endif  // HUSARION_UGV_LIGHTS_ANIMATION_ANIMATION_HPP_
diff --git a/panther_lights/include/panther_lights/animation/charging_animation.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/animation/charging_animation.hpp
similarity index 83%
rename from panther_lights/include/panther_lights/animation/charging_animation.hpp
rename to husarion_ugv_lights/include/husarion_ugv_lights/animation/charging_animation.hpp
index 95f7ab7b..2c325bc2 100644
--- a/panther_lights/include/panther_lights/animation/charging_animation.hpp
+++ b/husarion_ugv_lights/include/husarion_ugv_lights/animation/charging_animation.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_LIGHTS_ANIMATION_CHARGING_ANIMATION_HPP_
-#define PANTHER_LIGHTS_ANIMATION_CHARGING_ANIMATION_HPP_
+#ifndef HUSARION_UGV_LIGHTS_ANIMATION_CHARGING_ANIMATION_HPP_
+#define HUSARION_UGV_LIGHTS_ANIMATION_CHARGING_ANIMATION_HPP_
 
 #include <array>
 #include <cstdint>
@@ -22,9 +22,9 @@
 
 #include "yaml-cpp/yaml.h"
 
-#include "panther_lights/animation/animation.hpp"
+#include "husarion_ugv_lights/animation/animation.hpp"
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 class ChargingAnimation : public Animation
@@ -57,6 +57,6 @@ class ChargingAnimation : public Animation
   std::array<std::uint8_t, 3> color_;
 };
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
 
-#endif  // PANTHER_LIGHTS_ANIMATION_CHARGING_ANIMATION_HPP_
+#endif  // HUSARION_UGV_LIGHTS_ANIMATION_CHARGING_ANIMATION_HPP_
diff --git a/panther_lights/include/panther_lights/animation/image_animation.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/animation/image_animation.hpp
similarity index 88%
rename from panther_lights/include/panther_lights/animation/image_animation.hpp
rename to husarion_ugv_lights/include/husarion_ugv_lights/animation/image_animation.hpp
index 31663f95..78506908 100644
--- a/panther_lights/include/panther_lights/animation/image_animation.hpp
+++ b/husarion_ugv_lights/include/husarion_ugv_lights/animation/image_animation.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_LIGHTS_ANIMATION_IMAGE_ANIMATION_HPP_
-#define PANTHER_LIGHTS_ANIMATION_IMAGE_ANIMATION_HPP_
+#ifndef HUSARION_UGV_LIGHTS_ANIMATION_IMAGE_ANIMATION_HPP_
+#define HUSARION_UGV_LIGHTS_ANIMATION_IMAGE_ANIMATION_HPP_
 
 #include <cstdint>
 #include <filesystem>
@@ -25,11 +25,11 @@
 #include "boost/gil.hpp"
 #include "boost/gil/extension/toolbox/color_spaces/gray_alpha.hpp"
 
-#include "panther_lights/animation/animation.hpp"
+#include "husarion_ugv_lights/animation/animation.hpp"
 
 namespace gil = boost::gil;
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 class ImageAnimation : public Animation
@@ -77,6 +77,6 @@ class ImageAnimation : public Animation
   gil::rgba8_image_t image_;
 };
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
 
-#endif  // PANTHER_LIGHTS_ANIMATION_IMAGE_ANIMATION_HPP_
+#endif  // HUSARION_UGV_LIGHTS_ANIMATION_IMAGE_ANIMATION_HPP_
diff --git a/panther_lights/include/panther_lights/apa102.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/apa102.hpp
similarity index 96%
rename from panther_lights/include/panther_lights/apa102.hpp
rename to husarion_ugv_lights/include/husarion_ugv_lights/apa102.hpp
index 801a0610..f58ed240 100644
--- a/panther_lights/include/panther_lights/apa102.hpp
+++ b/husarion_ugv_lights/include/husarion_ugv_lights/apa102.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_LIGHTS_APA102_HPP_
-#define PANTHER_LIGHTS_APA102_HPP_
+#ifndef HUSARION_UGV_LIGHTS_APA102_HPP_
+#define HUSARION_UGV_LIGHTS_APA102_HPP_
 
 #include <fcntl.h>
 #include <linux/spi/spidev.h>
@@ -25,7 +25,7 @@
 #include <string>
 #include <vector>
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 class SPIDeviceInterface
@@ -165,6 +165,6 @@ class APA102 : public APA102Interface
   const int file_descriptor_;
 };
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
 
-#endif  // PANTHER_LIGHTS_APA102_HPP_
+#endif  // HUSARION_UGV_LIGHTS_APA102_HPP_
diff --git a/panther_lights/include/panther_lights/led_components/led_animations_queue.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_animations_queue.hpp
similarity index 94%
rename from panther_lights/include/panther_lights/led_components/led_animations_queue.hpp
rename to husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_animations_queue.hpp
index e9fd454d..6327bfd5 100644
--- a/panther_lights/include/panther_lights/led_components/led_animations_queue.hpp
+++ b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_animations_queue.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_LIGHTS_LED_COMPONENTS_LED_ANIMATIONS_QUEUE_HPP_
-#define PANTHER_LIGHTS_LED_COMPONENTS_LED_ANIMATIONS_QUEUE_HPP_
+#ifndef HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_ANIMATIONS_QUEUE_HPP_
+#define HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_ANIMATIONS_QUEUE_HPP_
 
 #include <array>
 #include <cstdint>
@@ -27,9 +27,9 @@
 
 #include "rclcpp/time.hpp"
 
-#include "panther_lights/led_components/led_segment.hpp"
+#include "husarion_ugv_lights/led_components/led_segment.hpp"
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 /**
@@ -189,6 +189,6 @@ class LEDAnimationsQueue
   const std::size_t max_queue_size_;
 };
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
 
-#endif  // PANTHER_LIGHTS_LED_COMPONENTS_LED_ANIMATIONS_QUEUE_HPP_
+#endif  // HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_ANIMATIONS_QUEUE_HPP_
diff --git a/panther_lights/include/panther_lights/led_components/led_panel.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_panel.hpp
similarity index 85%
rename from panther_lights/include/panther_lights/led_components/led_panel.hpp
rename to husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_panel.hpp
index 06d8def5..71719dcb 100644
--- a/panther_lights/include/panther_lights/led_components/led_panel.hpp
+++ b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_panel.hpp
@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_LIGHTS_LED_COMPONENTS_LED_PANEL_HPP_
-#define PANTHER_LIGHTS_LED_COMPONENTS_LED_PANEL_HPP_
+#ifndef HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_PANEL_HPP_
+#define HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_PANEL_HPP_
 
 #include <cstdint>
 #include <vector>
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 /**
@@ -49,6 +49,6 @@ class LEDPanel
   std::vector<std::uint8_t> frame_;
 };
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
 
-#endif  // PANTHER_LIGHTS_LED_COMPONENTS_LED_PANEL_HPP_
+#endif  // HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_PANEL_HPP_
diff --git a/panther_lights/include/panther_lights/led_components/led_segment.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_segment.hpp
similarity index 87%
rename from panther_lights/include/panther_lights/led_components/led_segment.hpp
rename to husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_segment.hpp
index 91b03724..5133d947 100644
--- a/panther_lights/include/panther_lights/led_components/led_segment.hpp
+++ b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_segment.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_LIGHTS_LED_COMPONENTS_LED_SEGMENT_HPP_
-#define PANTHER_LIGHTS_LED_COMPONENTS_LED_SEGMENT_HPP_
+#ifndef HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_SEGMENT_HPP_
+#define HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_SEGMENT_HPP_
 
 #include <cstdint>
 #include <vector>
@@ -22,9 +22,9 @@
 
 #include "pluginlib/class_loader.hpp"
 
-#include "panther_lights/animation/animation.hpp"
+#include "husarion_ugv_lights/animation/animation.hpp"
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 /**
@@ -117,8 +117,8 @@ class LEDSegment
   bool HasAnimation() const { return animation_ || default_animation_; }
 
 protected:
-  std::shared_ptr<panther_lights::Animation> animation_;
-  std::shared_ptr<panther_lights::Animation> default_animation_;
+  std::shared_ptr<husarion_ugv_lights::Animation> animation_;
+  std::shared_ptr<husarion_ugv_lights::Animation> default_animation_;
 
 private:
   const float controller_frequency_;
@@ -129,9 +129,9 @@ class LEDSegment
   std::size_t last_led_iterator_;
   std::size_t num_led_;
 
-  std::shared_ptr<pluginlib::ClassLoader<panther_lights::Animation>> animation_loader_;
+  std::shared_ptr<pluginlib::ClassLoader<husarion_ugv_lights::Animation>> animation_loader_;
 };
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
 
-#endif  // PANTHER_LIGHTS_LED_COMPONENTS_LED_SEGMENT_HPP_
+#endif  // HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_SEGMENT_HPP_
diff --git a/panther_lights/include/panther_lights/led_components/segment_converter.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/segment_converter.hpp
similarity index 70%
rename from panther_lights/include/panther_lights/led_components/segment_converter.hpp
rename to husarion_ugv_lights/include/husarion_ugv_lights/led_components/segment_converter.hpp
index fba745cd..1d23cbc8 100644
--- a/panther_lights/include/panther_lights/led_components/segment_converter.hpp
+++ b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/segment_converter.hpp
@@ -12,17 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_LIGHTS_LED_COMPONENTS_SEGMENT_CONVERTER_HPP_
-#define PANTHER_LIGHTS_LED_COMPONENTS_SEGMENT_CONVERTER_HPP_
+#ifndef HUSARION_UGV_LIGHTS_LED_COMPONENTS_SEGMENT_CONVERTER_HPP_
+#define HUSARION_UGV_LIGHTS_LED_COMPONENTS_SEGMENT_CONVERTER_HPP_
 
 #include <memory>
 #include <unordered_map>
 #include <vector>
 
-#include "panther_lights/led_components/led_panel.hpp"
-#include "panther_lights/led_components/led_segment.hpp"
+#include "husarion_ugv_lights/led_components/led_panel.hpp"
+#include "husarion_ugv_lights/led_components/led_segment.hpp"
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 class SegmentConverter
@@ -36,6 +36,6 @@ class SegmentConverter
     const std::unordered_map<std::size_t, std::shared_ptr<LEDPanel>> & panels);
 };
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
 
-#endif  // PANTHER_LIGHTS_LED_COMPONENTS_SEGMENT_CONVERTER_HPP_
+#endif  // HUSARION_UGV_LIGHTS_LED_COMPONENTS_SEGMENT_CONVERTER_HPP_
diff --git a/panther_lights/include/panther_lights/lights_controller_node.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp
similarity index 91%
rename from panther_lights/include/panther_lights/lights_controller_node.hpp
rename to husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp
index eb98f16f..fc7b61cc 100644
--- a/panther_lights/include/panther_lights/lights_controller_node.hpp
+++ b/husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_LIGHTS_LIGHTS_CONTROLLER_NODE_HPP_
-#define PANTHER_LIGHTS_LIGHTS_CONTROLLER_NODE_HPP_
+#ifndef HUSARION_UGV_LIGHTS_LIGHTS_CONTROLLER_NODE_HPP_
+#define HUSARION_UGV_LIGHTS_LIGHTS_CONTROLLER_NODE_HPP_
 
 #include <memory>
 #include <string>
@@ -27,12 +27,12 @@
 
 #include "panther_msgs/srv/set_led_animation.hpp"
 
-#include "panther_lights/animation/animation.hpp"
-#include "panther_lights/led_components/led_animations_queue.hpp"
-#include "panther_lights/led_components/segment_converter.hpp"
-#include "panther_utils/yaml_utils.hpp"
+#include "husarion_ugv_lights/animation/animation.hpp"
+#include "husarion_ugv_lights/led_components/led_animations_queue.hpp"
+#include "husarion_ugv_lights/led_components/segment_converter.hpp"
+#include "husarion_ugv_utils/yaml_utils.hpp"
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 using ImageMsg = sensor_msgs::msg::Image;
@@ -154,6 +154,6 @@ class LightsControllerNode : public rclcpp::Node
   bool animation_finished_ = true;
 };
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
 
-#endif  // PANTHER_LIGHTS_LIGHTS_CONTROLLER_NODE_HPP_
+#endif  // HUSARION_UGV_LIGHTS_LIGHTS_CONTROLLER_NODE_HPP_
diff --git a/panther_lights/include/panther_lights/lights_driver_node.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp
similarity index 94%
rename from panther_lights/include/panther_lights/lights_driver_node.hpp
rename to husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp
index f1a4b5b5..0a5de4ae 100644
--- a/panther_lights/include/panther_lights/lights_driver_node.hpp
+++ b/husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_LIGHTS_LIGHTS_DRIVER_NODE_HPP_
-#define PANTHER_LIGHTS_LIGHTS_DRIVER_NODE_HPP_
+#ifndef HUSARION_UGV_LIGHTS_LIGHTS_DRIVER_NODE_HPP_
+#define HUSARION_UGV_LIGHTS_LIGHTS_DRIVER_NODE_HPP_
 
 #include <memory>
 #include <string>
@@ -26,9 +26,9 @@
 
 #include "panther_msgs/srv/set_led_brightness.hpp"
 
-#include "panther_lights/apa102.hpp"
+#include "husarion_ugv_lights/apa102.hpp"
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 using ImageMsg = sensor_msgs::msg::Image;
@@ -133,6 +133,6 @@ class LightsDriverNode : public rclcpp::Node
   diagnostic_updater::Updater diagnostic_updater_;
 };
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
 
-#endif  // PANTHER_LIGHTS_LIGHTS_DRIVER_NODE_HPP_
+#endif  // HUSARION_UGV_LIGHTS_LIGHTS_DRIVER_NODE_HPP_
diff --git a/panther_lights/launch/lights.launch.py b/husarion_ugv_lights/launch/lights.launch.py
similarity index 93%
rename from panther_lights/launch/lights.launch.py
rename to husarion_ugv_lights/launch/lights.launch.py
index f77ef14d..fa45aa53 100644
--- a/panther_lights/launch/lights.launch.py
+++ b/husarion_ugv_lights/launch/lights.launch.py
@@ -33,7 +33,7 @@
 
 def generate_launch_description():
     robot_model = LaunchConfiguration("robot_model")
-    lights_pkg = FindPackageShare("panther_lights")
+    lights_pkg = FindPackageShare("husarion_ugv_lights")
     animations_config = PythonExpression(["'", robot_model, "_animations.yaml'"])
 
     animations_config_path = LaunchConfiguration("animations_config_path")
@@ -82,8 +82,8 @@ def generate_launch_description():
         executable="component_container",
         composable_node_descriptions=[
             ComposableNode(
-                package="panther_lights",
-                plugin="panther_lights::LightsDriverNode",
+                package="husarion_ugv_lights",
+                plugin="husarion_ugv_lights::LightsDriverNode",
                 name="lights_driver",
                 namespace=namespace,
                 remappings=[("/diagnostics", "diagnostics")],
@@ -94,8 +94,8 @@ def generate_launch_description():
                 condition=UnlessCondition(use_sim),
             ),
             ComposableNode(
-                package="panther_lights",
-                plugin="panther_lights::LightsControllerNode",
+                package="husarion_ugv_lights",
+                plugin="husarion_ugv_lights::LightsControllerNode",
                 name="lights_controller",
                 namespace=namespace,
                 parameters=[
diff --git a/panther_lights/package.xml b/husarion_ugv_lights/package.xml
similarity index 88%
rename from panther_lights/package.xml
rename to husarion_ugv_lights/package.xml
index 2dfb2248..8e68c10c 100644
--- a/panther_lights/package.xml
+++ b/husarion_ugv_lights/package.xml
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>panther_lights</name>
+  <name>husarion_ugv_lights</name>
   <version>2.1.1</version>
-  <description>Package used to control the Husarion Panther Bumper Lights</description>
+  <description>Package used to control the robot lights</description>
   <maintainer email="support@husarion.com">Husarion</maintainer>
   <license>Apache License 2.0</license>
 
@@ -17,10 +17,10 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>diagnostic_updater</depend>
+  <depend>husarion_ugv_utils</depend>
   <depend>image_transport</depend>
   <depend>libpng-dev</depend>
   <depend>panther_msgs</depend>
-  <depend>panther_utils</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
diff --git a/husarion_ugv_lights/plugins.xml b/husarion_ugv_lights/plugins.xml
new file mode 100644
index 00000000..5b499546
--- /dev/null
+++ b/husarion_ugv_lights/plugins.xml
@@ -0,0 +1,8 @@
+<library path="animation_plugins">
+  <class type="husarion_ugv_lights::ImageAnimation" base_class_type="husarion_ugv_lights::Animation">
+    <description>Animation processed from an image</description>
+  </class>
+  <class type="husarion_ugv_lights::ChargingAnimation" base_class_type="husarion_ugv_lights::Animation">
+    <description>Charging animation representing percentage of battery</description>
+  </class>
+</library>
diff --git a/panther_lights/src/animation/charging_animation.cpp b/husarion_ugv_lights/src/animation/charging_animation.cpp
similarity index 95%
rename from panther_lights/src/animation/charging_animation.cpp
rename to husarion_ugv_lights/src/animation/charging_animation.cpp
index 9a02d6b7..4aeec55d 100644
--- a/panther_lights/src/animation/charging_animation.cpp
+++ b/husarion_ugv_lights/src/animation/charging_animation.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_lights/animation/charging_animation.hpp"
+#include "husarion_ugv_lights/animation/charging_animation.hpp"
 
 #include <algorithm>
 #include <array>
@@ -25,7 +25,7 @@
 
 #include "yaml-cpp/yaml.h"
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 void ChargingAnimation::Initialize(
@@ -153,8 +153,8 @@ std::vector<std::uint8_t> ChargingAnimation::CreateRGBAFrame(
   return frame;
 }
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
 
 #include "pluginlib/class_list_macros.hpp"
 
-PLUGINLIB_EXPORT_CLASS(panther_lights::ChargingAnimation, panther_lights::Animation)
+PLUGINLIB_EXPORT_CLASS(husarion_ugv_lights::ChargingAnimation, husarion_ugv_lights::Animation)
diff --git a/panther_lights/src/animation/image_animation.cpp b/husarion_ugv_lights/src/animation/image_animation.cpp
similarity index 93%
rename from panther_lights/src/animation/image_animation.cpp
rename to husarion_ugv_lights/src/animation/image_animation.cpp
index 5cc68f08..ab7d9cd9 100644
--- a/panther_lights/src/animation/image_animation.cpp
+++ b/husarion_ugv_lights/src/animation/image_animation.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_lights/animation/image_animation.hpp"
+#include "husarion_ugv_lights/animation/image_animation.hpp"
 
 #include <algorithm>
 #include <cstdint>
@@ -30,9 +30,9 @@
 #include "boost/gil/extension/numeric/resample.hpp"
 #include "boost/gil/extension/numeric/sampler.hpp"
 
-#include "panther_utils/yaml_utils.hpp"
+#include "husarion_ugv_utils/yaml_utils.hpp"
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 void ImageAnimation::Initialize(
@@ -41,8 +41,8 @@ void ImageAnimation::Initialize(
 {
   Animation::Initialize(animation_description, num_led, controller_frequency);
 
-  const auto image_path =
-    ParseImagePath(panther_utils::GetYAMLKeyValue<std::string>(animation_description, "image"));
+  const auto image_path = ParseImagePath(
+    husarion_ugv_utils::GetYAMLKeyValue<std::string>(animation_description, "image"));
   gil::rgba8_image_t base_image;
   gil::read_and_convert_image(std::string(image_path), base_image, gil::png_tag());
   image_ = RGBAImageResize(base_image, this->GetNumberOfLeds(), this->GetAnimationLength());
@@ -156,8 +156,8 @@ void ImageAnimation::GreyImageNormalizeBrightness(gil::gray_alpha8_image_t & ima
     });
 }
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
 
 #include "pluginlib/class_list_macros.hpp"
 
-PLUGINLIB_EXPORT_CLASS(panther_lights::ImageAnimation, panther_lights::Animation)
+PLUGINLIB_EXPORT_CLASS(husarion_ugv_lights::ImageAnimation, husarion_ugv_lights::Animation)
diff --git a/panther_lights/src/apa102.cpp b/husarion_ugv_lights/src/apa102.cpp
similarity index 97%
rename from panther_lights/src/apa102.cpp
rename to husarion_ugv_lights/src/apa102.cpp
index 8a2a70aa..df35f857 100644
--- a/panther_lights/src/apa102.cpp
+++ b/husarion_ugv_lights/src/apa102.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_lights/apa102.hpp"
+#include "husarion_ugv_lights/apa102.hpp"
 
 #include <cmath>
 #include <cstdint>
@@ -22,7 +22,7 @@
 #include <string>
 #include <vector>
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 APA102::APA102(
@@ -126,4 +126,4 @@ void APA102::SPISendBuffer(const std::vector<std::uint8_t> & buffer) const
   }
 }
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
diff --git a/panther_lights/src/led_components/led_animations_queue.cpp b/husarion_ugv_lights/src/led_components/led_animations_queue.cpp
similarity index 96%
rename from panther_lights/src/led_components/led_animations_queue.cpp
rename to husarion_ugv_lights/src/led_components/led_animations_queue.cpp
index 0d295ab0..6a0aa04f 100644
--- a/panther_lights/src/led_components/led_animations_queue.cpp
+++ b/husarion_ugv_lights/src/led_components/led_animations_queue.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_lights/led_components/led_animations_queue.hpp"
+#include "husarion_ugv_lights/led_components/led_animations_queue.hpp"
 
 #include <functional>
 #include <memory>
@@ -23,7 +23,7 @@
 #include "rclcpp/logging.hpp"
 #include "rclcpp/time.hpp"
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 LEDAnimation::LEDAnimation(
@@ -148,4 +148,4 @@ bool LEDAnimationsQueue::HasAnimation(const std::shared_ptr<LEDAnimation> & anim
   return std::find(queue_.begin(), queue_.end(), animation) != queue_.end();
 }
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
diff --git a/panther_lights/src/led_components/led_panel.cpp b/husarion_ugv_lights/src/led_components/led_panel.cpp
similarity index 93%
rename from panther_lights/src/led_components/led_panel.cpp
rename to husarion_ugv_lights/src/led_components/led_panel.cpp
index 46820f17..6107e1ea 100644
--- a/panther_lights/src/led_components/led_panel.cpp
+++ b/husarion_ugv_lights/src/led_components/led_panel.cpp
@@ -12,14 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_lights/led_components/led_panel.hpp"
+#include "husarion_ugv_lights/led_components/led_panel.hpp"
 
 #include <algorithm>
 #include <cstdint>
 #include <stdexcept>
 #include <vector>
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 LEDPanel::LEDPanel(const std::size_t num_led) : num_led_(num_led)
@@ -50,4 +50,4 @@ void LEDPanel::UpdateFrame(
   std::copy(values.begin(), values.end(), frame_.begin() + iterator_first);
 }
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
diff --git a/panther_lights/src/led_components/led_segment.cpp b/husarion_ugv_lights/src/led_components/led_segment.cpp
similarity index 86%
rename from panther_lights/src/led_components/led_segment.cpp
rename to husarion_ugv_lights/src/led_components/led_segment.cpp
index bd4eaa98..ef82263d 100644
--- a/panther_lights/src/led_components/led_segment.cpp
+++ b/husarion_ugv_lights/src/led_components/led_segment.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_lights/led_components/led_segment.hpp"
+#include "husarion_ugv_lights/led_components/led_segment.hpp"
 
 #include <cmath>
 #include <cstdint>
@@ -20,17 +20,17 @@
 
 #include "yaml-cpp/yaml.h"
 
-#include "panther_lights/animation/animation.hpp"
-#include "panther_utils/yaml_utils.hpp"
+#include "husarion_ugv_lights/animation/animation.hpp"
+#include "husarion_ugv_utils/yaml_utils.hpp"
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 LEDSegment::LEDSegment(const YAML::Node & segment_description, const float controller_frequency)
 : controller_frequency_(controller_frequency)
 {
-  channel_ = panther_utils::GetYAMLKeyValue<std::size_t>(segment_description, "channel");
-  const auto led_range = panther_utils::GetYAMLKeyValue<std::string>(
+  channel_ = husarion_ugv_utils::GetYAMLKeyValue<std::size_t>(segment_description, "channel");
+  const auto led_range = husarion_ugv_utils::GetYAMLKeyValue<std::string>(
     segment_description, "led_range");
 
   const std::size_t split_char = led_range.find('-');
@@ -52,8 +52,8 @@ LEDSegment::LEDSegment(const YAML::Node & segment_description, const float contr
 
   num_led_ = std::abs(int(last_led_iterator_ - first_led_iterator_)) + 1;
 
-  animation_loader_ = std::make_shared<pluginlib::ClassLoader<panther_lights::Animation>>(
-    "panther_lights", "panther_lights::Animation");
+  animation_loader_ = std::make_shared<pluginlib::ClassLoader<husarion_ugv_lights::Animation>>(
+    "husarion_ugv_lights", "husarion_ugv_lights::Animation");
 }
 
 LEDSegment::~LEDSegment()
@@ -68,7 +68,7 @@ void LEDSegment::SetAnimation(
   const std::string & type, const YAML::Node & animation_description, const bool repeating,
   const std::string & param)
 {
-  std::shared_ptr<panther_lights::Animation> animation;
+  std::shared_ptr<husarion_ugv_lights::Animation> animation;
 
   try {
     animation = animation_loader_->createSharedInstance(type);
@@ -107,7 +107,7 @@ void LEDSegment::UpdateAnimation()
     animation_finished_ = true;
   }
 
-  std::shared_ptr<panther_lights::Animation> animation_to_update =
+  std::shared_ptr<husarion_ugv_lights::Animation> animation_to_update =
     animation_finished_ && default_animation_ ? default_animation_ : animation_;
 
   if (animation_finished_ && default_animation_ && default_animation_->IsFinished()) {
@@ -167,4 +167,4 @@ std::size_t LEDSegment::GetFirstLEDPosition() const
   return (invert_led_order_ ? last_led_iterator_ : first_led_iterator_) * Animation::kRGBAColorLen;
 }
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
diff --git a/panther_lights/src/led_components/segment_converter.cpp b/husarion_ugv_lights/src/led_components/segment_converter.cpp
similarity index 85%
rename from panther_lights/src/led_components/segment_converter.cpp
rename to husarion_ugv_lights/src/led_components/segment_converter.cpp
index fb1f54dd..b5ecf372 100644
--- a/panther_lights/src/led_components/segment_converter.cpp
+++ b/husarion_ugv_lights/src/led_components/segment_converter.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_lights/led_components/segment_converter.hpp"
+#include "husarion_ugv_lights/led_components/segment_converter.hpp"
 
 #include <memory>
 #include <stdexcept>
@@ -20,10 +20,10 @@
 #include <unordered_map>
 #include <vector>
 
-#include "panther_lights/led_components/led_panel.hpp"
-#include "panther_lights/led_components/led_segment.hpp"
+#include "husarion_ugv_lights/led_components/led_panel.hpp"
+#include "husarion_ugv_lights/led_components/led_segment.hpp"
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 void SegmentConverter::Convert(
@@ -52,4 +52,4 @@ void SegmentConverter::Convert(
   }
 }
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
diff --git a/panther_lights/src/lights_controller_node.cpp b/husarion_ugv_lights/src/lights_controller_node.cpp
similarity index 86%
rename from panther_lights/src/lights_controller_node.cpp
rename to husarion_ugv_lights/src/lights_controller_node.cpp
index d77443ef..b54cf25d 100644
--- a/panther_lights/src/lights_controller_node.cpp
+++ b/husarion_ugv_lights/src/lights_controller_node.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_lights/lights_controller_node.hpp"
+#include "husarion_ugv_lights/lights_controller_node.hpp"
 
 #include <chrono>
 #include <cstdint>
@@ -31,14 +31,14 @@
 
 #include "panther_msgs/srv/set_led_animation.hpp"
 
-#include "panther_lights/led_components/led_animations_queue.hpp"
-#include "panther_lights/led_components/led_panel.hpp"
-#include "panther_lights/led_components/led_segment.hpp"
-#include "panther_lights/led_components/segment_converter.hpp"
-#include "panther_utils/ros_utils.hpp"
-#include "panther_utils/yaml_utils.hpp"
+#include "husarion_ugv_lights/led_components/led_animations_queue.hpp"
+#include "husarion_ugv_lights/led_components/led_panel.hpp"
+#include "husarion_ugv_lights/led_components/led_segment.hpp"
+#include "husarion_ugv_lights/led_components/segment_converter.hpp"
+#include "husarion_ugv_utils/ros_utils.hpp"
+#include "husarion_ugv_utils/yaml_utils.hpp"
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 LightsControllerNode::LightsControllerNode(const rclcpp::NodeOptions & options)
@@ -86,8 +86,8 @@ void LightsControllerNode::InitializeLEDPanels(const YAML::Node & panels_descrip
   RCLCPP_DEBUG(this->get_logger(), "Initializing LED panels.");
 
   for (auto & panel : panels_description.as<std::vector<YAML::Node>>()) {
-    const auto channel = panther_utils::GetYAMLKeyValue<std::size_t>(panel, "channel");
-    const auto number_of_leds = panther_utils::GetYAMLKeyValue<std::size_t>(
+    const auto channel = husarion_ugv_utils::GetYAMLKeyValue<std::size_t>(panel, "channel");
+    const auto number_of_leds = husarion_ugv_utils::GetYAMLKeyValue<std::size_t>(
       panel, "number_of_leds");
 
     const auto result = led_panels_.emplace(channel, std::make_unique<LEDPanel>(number_of_leds));
@@ -115,7 +115,7 @@ void LightsControllerNode::InitializeLEDSegments(
   RCLCPP_DEBUG(this->get_logger(), "Initializing LED segments.");
 
   for (auto & segment : segments_description.as<std::vector<YAML::Node>>()) {
-    const auto segment_name = panther_utils::GetYAMLKeyValue<std::string>(segment, "name");
+    const auto segment_name = husarion_ugv_utils::GetYAMLKeyValue<std::string>(segment, "name");
 
     try {
       const auto result = segments_.emplace(
@@ -165,17 +165,17 @@ void LightsControllerNode::LoadUserAnimations(const std::string & user_led_anima
 
   try {
     YAML::Node user_led_animations = YAML::LoadFile(user_led_animations_path);
-    auto user_animations = panther_utils::GetYAMLKeyValue<std::vector<YAML::Node>>(
+    auto user_animations = husarion_ugv_utils::GetYAMLKeyValue<std::vector<YAML::Node>>(
       user_led_animations, "user_animations");
 
     for (auto & animation_description : user_animations) {
       try {
-        auto id = panther_utils::GetYAMLKeyValue<std::size_t>(animation_description, "id");
+        auto id = husarion_ugv_utils::GetYAMLKeyValue<std::size_t>(animation_description, "id");
         if (id < 20) {
           throw std::runtime_error("Animation ID must be greater than 19.");
         }
 
-        auto priority = panther_utils::GetYAMLKeyValue<std::size_t>(
+        auto priority = husarion_ugv_utils::GetYAMLKeyValue<std::size_t>(
           animation_description, "priority", LEDAnimation::kDefaultPriority);
         if (priority == 1) {
           throw std::runtime_error("User animation can not have priority 1.");
@@ -199,13 +199,13 @@ void LightsControllerNode::LoadAnimation(const YAML::Node & animation_descriptio
   LEDAnimationDescription led_animation_desc;
 
   try {
-    led_animation_desc.id = panther_utils::GetYAMLKeyValue<std::size_t>(
+    led_animation_desc.id = husarion_ugv_utils::GetYAMLKeyValue<std::size_t>(
       animation_description, "id");
-    led_animation_desc.name = panther_utils::GetYAMLKeyValue<std::string>(
+    led_animation_desc.name = husarion_ugv_utils::GetYAMLKeyValue<std::string>(
       animation_description, "name", "ANIMATION_" + std::to_string(led_animation_desc.id));
-    led_animation_desc.priority = panther_utils::GetYAMLKeyValue<std::uint8_t>(
+    led_animation_desc.priority = husarion_ugv_utils::GetYAMLKeyValue<std::uint8_t>(
       animation_description, "priority", LEDAnimation::kDefaultPriority);
-    led_animation_desc.timeout = panther_utils::GetYAMLKeyValue<float>(
+    led_animation_desc.timeout = husarion_ugv_utils::GetYAMLKeyValue<float>(
       animation_description, "timeout", LEDAnimation::kDefaultTimeout);
 
     if (
@@ -215,14 +215,15 @@ void LightsControllerNode::LoadAnimation(const YAML::Node & animation_descriptio
       throw std::runtime_error("Invalid LED animation priority.");
     }
 
-    auto animations = panther_utils::GetYAMLKeyValue<std::vector<YAML::Node>>(
+    auto animations = husarion_ugv_utils::GetYAMLKeyValue<std::vector<YAML::Node>>(
       animation_description, "animations");
     for (auto & animation : animations) {
       AnimationDescription animation_desc;
-      animation_desc.type = panther_utils::GetYAMLKeyValue<std::string>(animation, "type");
-      animation_desc.animation = panther_utils::GetYAMLKeyValue<YAML::Node>(animation, "animation");
+      animation_desc.type = husarion_ugv_utils::GetYAMLKeyValue<std::string>(animation, "type");
+      animation_desc.animation = husarion_ugv_utils::GetYAMLKeyValue<YAML::Node>(
+        animation, "animation");
 
-      auto segments_group = panther_utils::GetYAMLKeyValue<std::string>(animation, "segments");
+      auto segments_group = husarion_ugv_utils::GetYAMLKeyValue<std::string>(animation, "segments");
       animation_desc.segments = segments_map_.at(segments_group);
 
       led_animation_desc.animations.push_back(animation_desc);
@@ -258,7 +259,7 @@ void LightsControllerNode::PublishPanelFrame(const std::size_t channel)
   const auto number_of_leds = panel->GetNumberOfLeds();
 
   ImageMsg::UniquePtr image(new ImageMsg);
-  image->header.frame_id = panther_utils::ros::AddNamespaceToFrameID(
+  image->header.frame_id = husarion_ugv_utils::ros::AddNamespaceToFrameID(
     "lights_channel_" + std::to_string(channel) + "_link", std::string(this->get_namespace()));
   image->header.stamp = this->get_clock()->now();
   image->encoding = "rgba8";
@@ -370,7 +371,7 @@ void LightsControllerNode::SetLEDAnimation(const std::shared_ptr<LEDAnimation> &
   current_animation_ = std::move(led_animation);
 }
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
 
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(panther_lights::LightsControllerNode)
+RCLCPP_COMPONENTS_REGISTER_NODE(husarion_ugv_lights::LightsControllerNode)
diff --git a/panther_lights/src/lights_driver_node.cpp b/husarion_ugv_lights/src/lights_driver_node.cpp
similarity index 97%
rename from panther_lights/src/lights_driver_node.cpp
rename to husarion_ugv_lights/src/lights_driver_node.cpp
index 7535955c..62e89236 100644
--- a/panther_lights/src/lights_driver_node.cpp
+++ b/husarion_ugv_lights/src/lights_driver_node.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_lights/lights_driver_node.hpp"
+#include "husarion_ugv_lights/lights_driver_node.hpp"
 
 #include <cstdint>
 #include <filesystem>
@@ -31,9 +31,9 @@
 
 #include "panther_msgs/srv/set_led_brightness.hpp"
 
-#include "panther_lights/apa102.hpp"
+#include "husarion_ugv_lights/apa102.hpp"
 
-namespace panther_lights
+namespace husarion_ugv_lights
 {
 
 using std::placeholders::_1;
@@ -280,7 +280,7 @@ void LightsDriverNode::DiagnoseLights(diagnostic_updater::DiagnosticStatusWrappe
   status.summary(error_level, message);
 }
 
-}  // namespace panther_lights
+}  // namespace husarion_ugv_lights
 
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(panther_lights::LightsDriverNode)
+RCLCPP_COMPONENTS_REGISTER_NODE(husarion_ugv_lights::LightsDriverNode)
diff --git a/panther_lights/test/files/animation.png b/husarion_ugv_lights/test/files/animation.png
similarity index 100%
rename from panther_lights/test/files/animation.png
rename to husarion_ugv_lights/test/files/animation.png
diff --git a/panther_lights/test/integration/panther_lights.test.py b/husarion_ugv_lights/test/integration/husarion_ugv_lights.test.py
similarity index 95%
rename from panther_lights/test/integration/panther_lights.test.py
rename to husarion_ugv_lights/test/integration/husarion_ugv_lights.test.py
index b3cac0a1..906adb4b 100644
--- a/panther_lights/test/integration/panther_lights.test.py
+++ b/husarion_ugv_lights/test/integration/husarion_ugv_lights.test.py
@@ -17,9 +17,9 @@
 
 import unittest
 
+import husarion_ugv_utils.integration_test_utils as test_utils
 import launch
 import launch_testing
-import panther_utils.integration_test_utils as test_utils
 import rclpy
 import rclpy.qos
 from diagnostic_msgs.msg import DiagnosticArray
@@ -39,18 +39,18 @@ def generate_test_description():
     # TODO: Should be possibility to launch integration test for specific robot
     animations_config_path = (
         PathJoinSubstitution(
-            [FindPackageShare("panther_lights"), "config", "panther_animations.yaml"]
+            [FindPackageShare("husarion_ugv_lights"), "config", "panther_animations.yaml"]
         ),
     )
 
     lights_controller_node = Node(
-        package="panther_lights",
+        package="husarion_ugv_lights",
         executable="lights_controller_node",
         parameters=[{"animations_config_path": animations_config_path}],
     )
 
     lights_driver_node = Node(
-        package="panther_lights",
+        package="husarion_ugv_lights",
         executable="lights_driver_node",
     )
 
diff --git a/panther_lights/test/unit/animation/test_animation.cpp b/husarion_ugv_lights/test/unit/animation/test_animation.cpp
similarity index 98%
rename from panther_lights/test/unit/animation/test_animation.cpp
rename to husarion_ugv_lights/test/unit/animation/test_animation.cpp
index fb85d6a9..11e1f1bb 100644
--- a/panther_lights/test/unit/animation/test_animation.cpp
+++ b/husarion_ugv_lights/test/unit/animation/test_animation.cpp
@@ -20,9 +20,9 @@
 #include "gtest/gtest.h"
 #include "yaml-cpp/yaml.h"
 
-#include "panther_lights/animation/animation.hpp"
+#include "husarion_ugv_lights/animation/animation.hpp"
 
-class AnimationWrapper : public panther_lights::Animation
+class AnimationWrapper : public husarion_ugv_lights::Animation
 {
 public:
   AnimationWrapper() {}
diff --git a/panther_lights/test/unit/animation/test_charging_animation.cpp b/husarion_ugv_lights/test/unit/animation/test_charging_animation.cpp
similarity index 97%
rename from panther_lights/test/unit/animation/test_charging_animation.cpp
rename to husarion_ugv_lights/test/unit/animation/test_charging_animation.cpp
index 9a1c6d6a..b95eff58 100644
--- a/panther_lights/test/unit/animation/test_charging_animation.cpp
+++ b/husarion_ugv_lights/test/unit/animation/test_charging_animation.cpp
@@ -21,9 +21,9 @@
 #include "gtest/gtest.h"
 #include "yaml-cpp/yaml.h"
 
-#include "panther_lights/animation/charging_animation.hpp"
+#include "husarion_ugv_lights/animation/charging_animation.hpp"
 
-class ChargingAnimationWrapper : public panther_lights::ChargingAnimation
+class ChargingAnimationWrapper : public husarion_ugv_lights::ChargingAnimation
 {
 public:
   ChargingAnimationWrapper() {}
diff --git a/panther_lights/test/unit/animation/test_image_animation.cpp b/husarion_ugv_lights/test/unit/animation/test_image_animation.cpp
similarity index 94%
rename from panther_lights/test/unit/animation/test_image_animation.cpp
rename to husarion_ugv_lights/test/unit/animation/test_image_animation.cpp
index 2409a30c..5fdb993c 100644
--- a/panther_lights/test/unit/animation/test_image_animation.cpp
+++ b/husarion_ugv_lights/test/unit/animation/test_image_animation.cpp
@@ -24,11 +24,11 @@
 #include "gtest/gtest.h"
 #include "yaml-cpp/yaml.h"
 
-#include "panther_lights/animation/image_animation.hpp"
+#include "husarion_ugv_lights/animation/image_animation.hpp"
 
 namespace gil = boost::gil;
 
-class ImageAnimationWrapper : public panther_lights::ImageAnimation
+class ImageAnimationWrapper : public husarion_ugv_lights::ImageAnimation
 {
 public:
   ImageAnimationWrapper() {}
@@ -98,17 +98,17 @@ TEST_F(TestImageAnimation, ParseImagePath)
   EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error);
 
   // invalid substitution
-  image_path = "$(fin panther_lights)/test/files/animation.png";
+  image_path = "$(fin husarion_ugv_lights)/test/files/animation.png";
   EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error);
-  image_path = "$(find panther_lights/test/files/animation.png";
+  image_path = "$(find husarion_ugv_lights/test/files/animation.png";
   EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error);
 
   // following ones may not work if ROS package is not build or sourced
-  image_path = "$(find panther_lights)/test/files/animation.png";
+  image_path = "$(find husarion_ugv_lights)/test/files/animation.png";
   EXPECT_NO_THROW(animation_->ParseImagePath(image_path));
 
   // multiple spaces after find syntax
-  image_path = "$(find    panther_lights)/test/files/animation.png";
+  image_path = "$(find    husarion_ugv_lights)/test/files/animation.png";
   EXPECT_NO_THROW(animation_->ParseImagePath(image_path));
 }
 
diff --git a/panther_lights/test/unit/led_components/test_led_animation.cpp b/husarion_ugv_lights/test/unit/led_components/test_led_animation.cpp
similarity index 79%
rename from panther_lights/test/unit/led_components/test_led_animation.cpp
rename to husarion_ugv_lights/test/unit/led_components/test_led_animation.cpp
index 51dbc58d..67173da0 100644
--- a/panther_lights/test/unit/led_components/test_led_animation.cpp
+++ b/husarion_ugv_lights/test/unit/led_components/test_led_animation.cpp
@@ -22,8 +22,8 @@
 
 #include "rclcpp/time.hpp"
 
-#include "panther_lights/led_components/led_animations_queue.hpp"
-#include "panther_lights/led_components/led_segment.hpp"
+#include "husarion_ugv_lights/led_components/led_animations_queue.hpp"
+#include "husarion_ugv_lights/led_components/led_segment.hpp"
 
 class TestLEDAnimation : public testing::Test
 {
@@ -37,8 +37,8 @@ class TestLEDAnimation : public testing::Test
   static constexpr char kTestSegmentName1[] = "segment_1";
   static constexpr char kTestSegmentName2[] = "segment_2";
 
-  std::shared_ptr<panther_lights::LEDAnimation> led_anim_;
-  std::unordered_map<std::string, std::shared_ptr<panther_lights::LEDSegment>> segments_;
+  std::shared_ptr<husarion_ugv_lights::LEDAnimation> led_anim_;
+  std::unordered_map<std::string, std::shared_ptr<husarion_ugv_lights::LEDSegment>> segments_;
 };
 
 TestLEDAnimation::TestLEDAnimation()
@@ -46,24 +46,24 @@ TestLEDAnimation::TestLEDAnimation()
   auto segment_1_desc = YAML::Load("{channel: 1, led_range: 0-10}");
   auto segment_2_desc = YAML::Load("{channel: 2, led_range: 0-10}");
   segments_.emplace(
-    kTestSegmentName1, std::make_shared<panther_lights::LEDSegment>(segment_1_desc, 50.0));
+    kTestSegmentName1, std::make_shared<husarion_ugv_lights::LEDSegment>(segment_1_desc, 50.0));
   segments_.emplace(
-    kTestSegmentName2, std::make_shared<panther_lights::LEDSegment>(segment_2_desc, 50.0));
+    kTestSegmentName2, std::make_shared<husarion_ugv_lights::LEDSegment>(segment_2_desc, 50.0));
 
-  panther_lights::AnimationDescription anim_desc;
+  husarion_ugv_lights::AnimationDescription anim_desc;
   anim_desc.segments = {kTestSegmentName1, kTestSegmentName2};
-  anim_desc.type = "panther_lights::ImageAnimation";
+  anim_desc.type = "husarion_ugv_lights::ImageAnimation";
   anim_desc.animation =
-    YAML::Load("{image: $(find panther_lights)/test/files/animation.png, duration: 2.0}");
+    YAML::Load("{image: $(find husarion_ugv_lights)/test/files/animation.png, duration: 2.0}");
 
-  panther_lights::LEDAnimationDescription led_anim_desc;
+  husarion_ugv_lights::LEDAnimationDescription led_anim_desc;
   led_anim_desc.id = 0;
   led_anim_desc.name = "TEST";
   led_anim_desc.priority = 1;
   led_anim_desc.timeout = 10.0;
   led_anim_desc.animations = {anim_desc};
 
-  led_anim_ = std::make_shared<panther_lights::LEDAnimation>(
+  led_anim_ = std::make_shared<husarion_ugv_lights::LEDAnimation>(
     led_anim_desc, segments_, rclcpp::Time(0));
 }
 
@@ -80,16 +80,16 @@ void TestLEDAnimation::SetSegmentAnimations()
 
 TEST(TestLEDAnimationInitialization, InvalidSegmentName)
 {
-  std::unordered_map<std::string, std::shared_ptr<panther_lights::LEDSegment>> segments;
+  std::unordered_map<std::string, std::shared_ptr<husarion_ugv_lights::LEDSegment>> segments;
 
-  panther_lights::AnimationDescription anim_desc;
+  husarion_ugv_lights::AnimationDescription anim_desc;
   anim_desc.segments = {"invalid_segment"};
 
-  panther_lights::LEDAnimationDescription led_anim_desc;
+  husarion_ugv_lights::LEDAnimationDescription led_anim_desc;
   led_anim_desc.animations = {anim_desc};
 
   EXPECT_THROW(
-    std::make_shared<panther_lights::LEDAnimation>(led_anim_desc, segments, rclcpp::Time(0)),
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(led_anim_desc, segments, rclcpp::Time(0)),
     std::runtime_error);
 }
 
diff --git a/panther_lights/test/unit/led_components/test_led_animations_queue.cpp b/husarion_ugv_lights/test/unit/led_components/test_led_animations_queue.cpp
similarity index 66%
rename from panther_lights/test/unit/led_components/test_led_animations_queue.cpp
rename to husarion_ugv_lights/test/unit/led_components/test_led_animations_queue.cpp
index 4eb8d8c7..d7d8c04a 100644
--- a/panther_lights/test/unit/led_components/test_led_animations_queue.cpp
+++ b/husarion_ugv_lights/test/unit/led_components/test_led_animations_queue.cpp
@@ -22,8 +22,8 @@
 
 #include "rclcpp/time.hpp"
 
-#include "panther_lights/led_components/led_animations_queue.hpp"
-#include "panther_lights/led_components/led_segment.hpp"
+#include "husarion_ugv_lights/led_components/led_animations_queue.hpp"
+#include "husarion_ugv_lights/led_components/led_segment.hpp"
 
 class TestLEDAnimationsQueue : public testing::Test
 {
@@ -31,13 +31,13 @@ class TestLEDAnimationsQueue : public testing::Test
   TestLEDAnimationsQueue();
   ~TestLEDAnimationsQueue() {}
 
-  panther_lights::LEDAnimation CreateLEDAnimation(
+  husarion_ugv_lights::LEDAnimation CreateLEDAnimation(
     const std::string & name, const std::uint8_t priority,
     const rclcpp::Time & init_time = rclcpp::Time(0));
 
 protected:
-  std::shared_ptr<panther_lights::LEDAnimationsQueue> led_anim_queue_;
-  std::unordered_map<std::string, std::shared_ptr<panther_lights::LEDSegment>> segments_;
+  std::shared_ptr<husarion_ugv_lights::LEDAnimationsQueue> led_anim_queue_;
+  std::unordered_map<std::string, std::shared_ptr<husarion_ugv_lights::LEDSegment>> segments_;
 
   const std::size_t max_queue_size_ = 5;
 };
@@ -47,35 +47,36 @@ TestLEDAnimationsQueue::TestLEDAnimationsQueue()
   auto segment_1_desc = YAML::Load("{channel: 1, led_range: 0-10}");
   auto segment_2_desc = YAML::Load("{channel: 2, led_range: 0-10}");
   segments_.emplace(
-    "segment_1", std::make_shared<panther_lights::LEDSegment>(segment_1_desc, 50.0));
+    "segment_1", std::make_shared<husarion_ugv_lights::LEDSegment>(segment_1_desc, 50.0));
   segments_.emplace(
-    "segment_2", std::make_shared<panther_lights::LEDSegment>(segment_2_desc, 50.0));
+    "segment_2", std::make_shared<husarion_ugv_lights::LEDSegment>(segment_2_desc, 50.0));
 
-  led_anim_queue_ = std::make_shared<panther_lights::LEDAnimationsQueue>(5);
+  led_anim_queue_ = std::make_shared<husarion_ugv_lights::LEDAnimationsQueue>(5);
 }
 
-panther_lights::LEDAnimation TestLEDAnimationsQueue::CreateLEDAnimation(
+husarion_ugv_lights::LEDAnimation TestLEDAnimationsQueue::CreateLEDAnimation(
   const std::string & name, const std::uint8_t priority, const rclcpp::Time & init_time)
 {
-  panther_lights::AnimationDescription anim_desc;
+  husarion_ugv_lights::AnimationDescription anim_desc;
   anim_desc.segments = {"segment_1", "segment_2"};
-  anim_desc.type = "panther_lights::ImageAnimation";
+  anim_desc.type = "husarion_ugv_lights::ImageAnimation";
   anim_desc.animation =
-    YAML::Load("{image: $(find panther_lights)/test/files/animation.png, duration: 2.0}");
+    YAML::Load("{image: $(find husarion_ugv_lights)/test/files/animation.png, duration: 2.0}");
 
-  panther_lights::LEDAnimationDescription led_anim_desc;
+  husarion_ugv_lights::LEDAnimationDescription led_anim_desc;
   led_anim_desc.id = 0;
   led_anim_desc.name = name;
   led_anim_desc.priority = priority;
   led_anim_desc.timeout = 10.0;
   led_anim_desc.animations = {anim_desc};
 
-  return panther_lights::LEDAnimation(led_anim_desc, segments_, init_time);
+  return husarion_ugv_lights::LEDAnimation(led_anim_desc, segments_, init_time);
 }
 
 TEST_F(TestLEDAnimationsQueue, Put)
 {
-  auto led_anim = std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST", 1));
+  auto led_anim =
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST", 1));
   led_anim_queue_->Put(led_anim, rclcpp::Time(0));
 
   EXPECT_FALSE(led_anim_queue_->Empty());
@@ -84,7 +85,8 @@ TEST_F(TestLEDAnimationsQueue, Put)
 
 TEST_F(TestLEDAnimationsQueue, PutQueueOverloaded)
 {
-  auto led_anim = std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST", 1));
+  auto led_anim =
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST", 1));
   for (std::size_t i = 0; i < max_queue_size_; i++) {
     led_anim_queue_->Put(led_anim, rclcpp::Time(0));
   }
@@ -95,11 +97,11 @@ TEST_F(TestLEDAnimationsQueue, PutQueueOverloaded)
 TEST_F(TestLEDAnimationsQueue, PutClearWhenPriorityEqualOne)
 {
   auto led_anim_pr_1 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST", 1));
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST", 1));
   auto led_anim_pr_2 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST", 2));
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST", 2));
   auto led_anim_pr_3 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST", 3));
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST", 3));
 
   led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0));
   led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0));
@@ -113,9 +115,9 @@ TEST_F(TestLEDAnimationsQueue, PutClearWhenPriorityEqualOne)
 TEST_F(TestLEDAnimationsQueue, PutSortByPriority)
 {
   auto led_anim_pr_2 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST", 2));
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST", 2));
   auto led_anim_pr_3 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST", 3));
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST", 3));
 
   led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0));
   led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0));
@@ -130,14 +132,14 @@ TEST_F(TestLEDAnimationsQueue, PutSortByPriority)
 
 TEST_F(TestLEDAnimationsQueue, PutSortByTime)
 {
-  auto led_anim_t0 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST", 1, rclcpp::Time(0)));
-  auto led_anim_t1 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST", 1, rclcpp::Time(1)));
-  auto led_anim_t2 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST", 1, rclcpp::Time(2)));
-  auto led_anim_t3 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST", 1, rclcpp::Time(3)));
+  auto led_anim_t0 = std::make_shared<husarion_ugv_lights::LEDAnimation>(
+    CreateLEDAnimation("TEST", 1, rclcpp::Time(0)));
+  auto led_anim_t1 = std::make_shared<husarion_ugv_lights::LEDAnimation>(
+    CreateLEDAnimation("TEST", 1, rclcpp::Time(1)));
+  auto led_anim_t2 = std::make_shared<husarion_ugv_lights::LEDAnimation>(
+    CreateLEDAnimation("TEST", 1, rclcpp::Time(2)));
+  auto led_anim_t3 = std::make_shared<husarion_ugv_lights::LEDAnimation>(
+    CreateLEDAnimation("TEST", 1, rclcpp::Time(3)));
 
   led_anim_queue_->Put(led_anim_t3, rclcpp::Time(4));
   led_anim_queue_->Put(led_anim_t1, rclcpp::Time(4));
@@ -158,11 +160,11 @@ TEST_F(TestLEDAnimationsQueue, GetQueueEmpty)
 TEST_F(TestLEDAnimationsQueue, Clear)
 {
   auto led_anim_pr_1 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST1", 1));
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST1", 1));
   auto led_anim_pr_2 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST2", 2));
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST2", 2));
   auto led_anim_pr_3 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST3", 3));
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST3", 3));
 
   led_anim_queue_->Put(led_anim_pr_1, rclcpp::Time(0));
   led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0));
@@ -184,8 +186,8 @@ TEST_F(TestLEDAnimationsQueue, Clear)
 
 TEST_F(TestLEDAnimationsQueue, ValidateAnimationTimedOut)
 {
-  auto led_anim =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST", 1, rclcpp::Time(0)));
+  auto led_anim = std::make_shared<husarion_ugv_lights::LEDAnimation>(
+    CreateLEDAnimation("TEST", 1, rclcpp::Time(0)));
 
   led_anim_queue_->Put(led_anim, rclcpp::Time(0));
   led_anim_queue_->Validate(rclcpp::Time(0));
@@ -200,11 +202,11 @@ TEST_F(TestLEDAnimationsQueue, ValidateAnimationTimedOut)
 TEST_F(TestLEDAnimationsQueue, GetFirstAnimationPriority)
 {
   auto led_anim_pr_1 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST1", 1));
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST1", 1));
   auto led_anim_pr_2 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST2", 2));
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST2", 2));
   auto led_anim_pr_3 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST3", 3));
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST3", 3));
 
   led_anim_queue_->Put(led_anim_pr_1, rclcpp::Time(0));
   led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0));
@@ -223,11 +225,11 @@ TEST_F(TestLEDAnimationsQueue, GetFirstAnimationPriority)
 TEST_F(TestLEDAnimationsQueue, Remove)
 {
   auto led_anim_pr_1 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST1", 1));
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST1", 1));
   auto led_anim_pr_2 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST2", 2));
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST2", 2));
   auto led_anim_pr_3 =
-    std::make_shared<panther_lights::LEDAnimation>(CreateLEDAnimation("TEST3", 3));
+    std::make_shared<husarion_ugv_lights::LEDAnimation>(CreateLEDAnimation("TEST3", 3));
 
   led_anim_queue_->Put(led_anim_pr_1, rclcpp::Time(0));
   led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0));
diff --git a/panther_lights/test/unit/led_components/test_led_panel.cpp b/husarion_ugv_lights/test/unit/led_components/test_led_panel.cpp
similarity index 93%
rename from panther_lights/test/unit/led_components/test_led_panel.cpp
rename to husarion_ugv_lights/test/unit/led_components/test_led_panel.cpp
index 3b3c1aaf..7e0c90c3 100644
--- a/panther_lights/test/unit/led_components/test_led_panel.cpp
+++ b/husarion_ugv_lights/test/unit/led_components/test_led_panel.cpp
@@ -19,18 +19,18 @@
 
 #include "gtest/gtest.h"
 
-#include "panther_lights/led_components/led_panel.hpp"
+#include "husarion_ugv_lights/led_components/led_panel.hpp"
 
 class TestLEDPanel : public testing::Test
 {
 public:
-  TestLEDPanel() { led_panel_ = std::make_unique<panther_lights::LEDPanel>(num_led_); }
+  TestLEDPanel() { led_panel_ = std::make_unique<husarion_ugv_lights::LEDPanel>(num_led_); }
   ~TestLEDPanel() {}
 
 protected:
   void UpdateAndTestFrame(
     const std::size_t iterator_first, const std::vector<std::uint8_t> & test_frame);
-  std::unique_ptr<panther_lights::LEDPanel> led_panel_;
+  std::unique_ptr<husarion_ugv_lights::LEDPanel> led_panel_;
 
   const std::size_t num_led_ = 46;
   const std::size_t frame_size_ = num_led_ * 4;
@@ -60,7 +60,7 @@ void TestLEDPanel::UpdateAndTestFrame(
 TEST(TestLEDPanelInitialization, FrameSize)
 {
   const std::size_t num_led = 22;
-  auto led_panel = panther_lights::LEDPanel(num_led);
+  auto led_panel = husarion_ugv_lights::LEDPanel(num_led);
   EXPECT_EQ(num_led * 4, led_panel.GetFrame().size());
 }
 
diff --git a/panther_lights/test/unit/led_components/test_led_segment.cpp b/husarion_ugv_lights/test/unit/led_components/test_led_segment.cpp
similarity index 57%
rename from panther_lights/test/unit/led_components/test_led_segment.cpp
rename to husarion_ugv_lights/test/unit/led_components/test_led_segment.cpp
index 82fbec3e..9423f2fd 100644
--- a/panther_lights/test/unit/led_components/test_led_segment.cpp
+++ b/husarion_ugv_lights/test/unit/led_components/test_led_segment.cpp
@@ -21,10 +21,10 @@
 #include "gtest/gtest.h"
 #include "yaml-cpp/yaml.h"
 
-#include "panther_lights/led_components/led_segment.hpp"
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_lights/led_components/led_segment.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
-class LEDSegmentWrapper : public panther_lights::LEDSegment
+class LEDSegmentWrapper : public husarion_ugv_lights::LEDSegment
 {
 public:
   LEDSegmentWrapper(const YAML::Node & segment_description, const float controller_frequency)
@@ -32,8 +32,8 @@ class LEDSegmentWrapper : public panther_lights::LEDSegment
   {
   }
 
-  std::shared_ptr<panther_lights::Animation> GetAnimation() const { return animation_; }
-  std::shared_ptr<panther_lights::Animation> GetDefaultAnimation() const
+  std::shared_ptr<husarion_ugv_lights::Animation> GetAnimation() const { return animation_; }
+  std::shared_ptr<husarion_ugv_lights::Animation> GetDefaultAnimation() const
   {
     return default_animation_;
   }
@@ -67,121 +67,128 @@ YAML::Node CreateSegmentDescription(const std::string & led_range, const std::st
 TEST(TestLEDSegmentInitialization, DescriptionMissingRequiredKey)
 {
   auto segment_desc = YAML::Load("");
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); },
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [segment_desc]() { husarion_ugv_lights::LEDSegment(segment_desc, 10.0); },
     "Missing 'channel' in description"));
 
   segment_desc = YAML::Load("channel: 0");
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); },
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [segment_desc]() { husarion_ugv_lights::LEDSegment(segment_desc, 10.0); },
     "Missing 'led_range' in description"));
 }
 
 TEST(TestLEDSegmentInitialization, InvalidChannelExpression)
 {
   auto segment_desc = CreateSegmentDescription("0-10", "s1");
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); },
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [segment_desc]() { husarion_ugv_lights::LEDSegment(segment_desc, 10.0); },
     "Failed to convert 'channel' key"));
 
   segment_desc["channel"] = "-1";
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); },
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [segment_desc]() { husarion_ugv_lights::LEDSegment(segment_desc, 10.0); },
     "Failed to convert 'channel' key"));
 }
 
 TEST(TestLEDSegmentInitialization, InvalidLedRangeExpression)
 {
   auto segment_desc = CreateSegmentDescription("010", "1");
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::invalid_argument>(
-    [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); },
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::invalid_argument>(
+    [segment_desc]() { husarion_ugv_lights::LEDSegment(segment_desc, 10.0); },
     "No '-' character found in the led_range expression"));
 
   segment_desc["led_range"] = "s0-10";
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::invalid_argument>(
-    [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); },
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::invalid_argument>(
+    [segment_desc]() { husarion_ugv_lights::LEDSegment(segment_desc, 10.0); },
     "Error converting string to integer"));
 
   segment_desc["led_range"] = "0-p10";
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::invalid_argument>(
-    [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); },
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::invalid_argument>(
+    [segment_desc]() { husarion_ugv_lights::LEDSegment(segment_desc, 10.0); },
     "Error converting string to integer"));
 }
 
 TEST(TestLEDSegmentInitialization, ValidDescription)
 {
   const auto segment_desc = CreateSegmentDescription("0-10", "1");
-  EXPECT_NO_THROW(panther_lights::LEDSegment(segment_desc, 10.0));
+  EXPECT_NO_THROW(husarion_ugv_lights::LEDSegment(segment_desc, 10.0));
 }
 
 TEST(TestLEDSegmentInitialization, FirstLedPosition)
 {
   auto segment_desc = CreateSegmentDescription("0-10", "1");
-  std::shared_ptr<panther_lights::LEDSegment> led_segment;
+  std::shared_ptr<husarion_ugv_lights::LEDSegment> led_segment;
 
-  ASSERT_NO_THROW(led_segment = std::make_shared<panther_lights::LEDSegment>(segment_desc, 10.0));
+  ASSERT_NO_THROW(
+    led_segment = std::make_shared<husarion_ugv_lights::LEDSegment>(segment_desc, 10.0));
   EXPECT_EQ(std::size_t(0), led_segment->GetFirstLEDPosition());
 
   segment_desc["led_range"] = "5-11";
   led_segment.reset();
-  ASSERT_NO_THROW(led_segment = std::make_shared<panther_lights::LEDSegment>(segment_desc, 10.0));
+  ASSERT_NO_THROW(
+    led_segment = std::make_shared<husarion_ugv_lights::LEDSegment>(segment_desc, 10.0));
   EXPECT_EQ(std::size_t(5 * 4), led_segment->GetFirstLEDPosition());
 
   segment_desc["led_range"] = "10-10";
   led_segment.reset();
-  ASSERT_NO_THROW(led_segment = std::make_shared<panther_lights::LEDSegment>(segment_desc, 10.0));
+  ASSERT_NO_THROW(
+    led_segment = std::make_shared<husarion_ugv_lights::LEDSegment>(segment_desc, 10.0));
   EXPECT_EQ(std::size_t(10 * 4), led_segment->GetFirstLEDPosition());
 
   segment_desc["led_range"] = "13-5";
   led_segment.reset();
-  ASSERT_NO_THROW(led_segment = std::make_shared<panther_lights::LEDSegment>(segment_desc, 10.0));
+  ASSERT_NO_THROW(
+    led_segment = std::make_shared<husarion_ugv_lights::LEDSegment>(segment_desc, 10.0));
   EXPECT_EQ(std::size_t(5 * 4), led_segment->GetFirstLEDPosition());
 
   segment_desc["led_range"] = "17-0";
   led_segment.reset();
-  ASSERT_NO_THROW(led_segment = std::make_shared<panther_lights::LEDSegment>(segment_desc, 10.0));
+  ASSERT_NO_THROW(
+    led_segment = std::make_shared<husarion_ugv_lights::LEDSegment>(segment_desc, 10.0));
   EXPECT_EQ(std::size_t(0), led_segment->GetFirstLEDPosition());
 }
 
 TEST_F(TestLEDSegment, GetAnimationFrameNoAnimation)
 {
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { led_segment_->GetAnimationFrame(); }, "Segment animation not defined"));
 }
 
 TEST_F(TestLEDSegment, GetAnimationProgressNoAnimation)
 {
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { led_segment_->GetAnimationProgress(); }, "Segment animation not defined"));
 }
 
 TEST_F(TestLEDSegment, ResetAnimationNoAnimation)
 {
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { led_segment_->ResetAnimation(); }, "Segment animation not defined"));
 }
 
 TEST_F(TestLEDSegment, GetAnimationBrightnessNoAnimation)
 {
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { led_segment_->GetAnimationBrightness(); }, "Segment animation not defined"));
 }
 
 TEST_F(TestLEDSegment, SetAnimationInvalidType)
 {
   const YAML::Node animation_desc;
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() {
-      led_segment_->SetAnimation("panther_lights::WrongAnimationType}", animation_desc, false);
+      led_segment_->SetAnimation("husarion_ugv_lights::WrongAnimationType}", animation_desc, false);
     },
     "The plugin failed to load. Error: "));
 }
 
 TEST_F(TestLEDSegment, SetAnimationFailAnimationInitialization)
 {
-  const auto animation_desc = YAML::Load("{type: panther_lights::ImageAnimation}");
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [&]() { led_segment_->SetAnimation("panther_lights::ImageAnimation", animation_desc, false); },
+  const auto animation_desc = YAML::Load("{type: husarion_ugv_lights::ImageAnimation}");
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() {
+      led_segment_->SetAnimation("husarion_ugv_lights::ImageAnimation", animation_desc, false);
+    },
     "Failed to initialize animation: "));
 }
 
@@ -189,27 +196,29 @@ TEST_F(TestLEDSegment, SetAnimation)
 {
   // test each known animtion type
   const auto image_anim_desc = YAML::Load(
-    "{image: $(find panther_lights)/test/files/animation.png, "
+    "{image: $(find husarion_ugv_lights)/test/files/animation.png, "
     "duration: 2}");
   const auto charging_anim_desc = YAML::Load("{duration: 2}");
 
   EXPECT_NO_THROW(
-    led_segment_->SetAnimation("panther_lights::ImageAnimation", image_anim_desc, false));
+    led_segment_->SetAnimation("husarion_ugv_lights::ImageAnimation", image_anim_desc, false));
 
   EXPECT_NO_THROW(led_segment_->SetAnimation(
-    "panther_lights::ChargingAnimation", charging_anim_desc, false, "0.5"));
+    "husarion_ugv_lights::ChargingAnimation", charging_anim_desc, false, "0.5"));
 }
 
 TEST_F(TestLEDSegment, SetAnimationRepeating)
 {
   const auto anim_desc = YAML::Load(
-    "{image: $(find panther_lights)/test/files/animation.png, "
+    "{image: $(find husarion_ugv_lights)/test/files/animation.png, "
     "duration: 2}");
-  ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, false));
+  ASSERT_NO_THROW(
+    led_segment_->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false));
 
   EXPECT_TRUE(led_segment_->GetDefaultAnimation().get() == nullptr);
 
-  ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, true));
+  ASSERT_NO_THROW(
+    led_segment_->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, true));
 
   EXPECT_TRUE(led_segment_->GetDefaultAnimation().get() != nullptr);
   EXPECT_TRUE(led_segment_->IsAnimationFinished());
@@ -217,16 +226,17 @@ TEST_F(TestLEDSegment, SetAnimationRepeating)
 
 TEST_F(TestLEDSegment, UpdateAnimationAnimationNotSet)
 {
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { led_segment_->UpdateAnimation(); }, "Segment animation not defined"));
 }
 
 TEST_F(TestLEDSegment, UpdateAnimation)
 {
   const auto anim_desc = YAML::Load(
-    "{image: $(find panther_lights)/test/files/animation.png, "
+    "{image: $(find husarion_ugv_lights)/test/files/animation.png, "
     "duration: 2}");
-  ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, false));
+  ASSERT_NO_THROW(
+    led_segment_->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false));
   EXPECT_NO_THROW(led_segment_->UpdateAnimation());
   EXPECT_EQ(segment_led_num_ * 4, led_segment_->GetAnimationFrame().size());
 }
@@ -240,9 +250,10 @@ int main(int argc, char ** argv)
 TEST_F(TestLEDSegment, ResetDefaultAnimationWhenNewArrive)
 {
   const auto anim_desc = YAML::Load(
-    "{image: $(find panther_lights)/test/files/animation.png, "
+    "{image: $(find husarion_ugv_lights)/test/files/animation.png, "
     "duration: 2}");
-  ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, true));
+  ASSERT_NO_THROW(
+    led_segment_->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, true));
 
   auto default_anim = led_segment_->GetDefaultAnimation();
   while (!default_anim->IsFinished()) {
@@ -250,7 +261,8 @@ TEST_F(TestLEDSegment, ResetDefaultAnimationWhenNewArrive)
   }
 
   // add new animation, and check if default animation was reset
-  ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, false));
+  ASSERT_NO_THROW(
+    led_segment_->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false));
 
   EXPECT_FALSE(default_anim->IsFinished());
 }
diff --git a/panther_lights/test/unit/led_components/test_segment_converter.cpp b/husarion_ugv_lights/test/unit/led_components/test_segment_converter.cpp
similarity index 69%
rename from panther_lights/test/unit/led_components/test_segment_converter.cpp
rename to husarion_ugv_lights/test/unit/led_components/test_segment_converter.cpp
index 807f9440..53928704 100644
--- a/panther_lights/test/unit/led_components/test_segment_converter.cpp
+++ b/husarion_ugv_lights/test/unit/led_components/test_segment_converter.cpp
@@ -22,20 +22,20 @@
 #include "gtest/gtest.h"
 #include "yaml-cpp/yaml.h"
 
-#include "panther_lights/led_components/led_panel.hpp"
-#include "panther_lights/led_components/led_segment.hpp"
-#include "panther_lights/led_components/segment_converter.hpp"
+#include "husarion_ugv_lights/led_components/led_panel.hpp"
+#include "husarion_ugv_lights/led_components/led_segment.hpp"
+#include "husarion_ugv_lights/led_components/segment_converter.hpp"
 
 class TestSegmentConverter : public testing::Test
 {
 public:
   TestSegmentConverter()
   {
-    segment_converter_ = std::make_unique<panther_lights::SegmentConverter>();
+    segment_converter_ = std::make_unique<husarion_ugv_lights::SegmentConverter>();
 
     // create 2 basic panels with different number of leds
-    led_panels_.insert({1, std::make_unique<panther_lights::LEDPanel>(panel_1_num_led_)});
-    led_panels_.insert({2, std::make_unique<panther_lights::LEDPanel>(panel_2_num_led_)});
+    led_panels_.insert({1, std::make_unique<husarion_ugv_lights::LEDPanel>(panel_1_num_led_)});
+    led_panels_.insert({2, std::make_unique<husarion_ugv_lights::LEDPanel>(panel_2_num_led_)});
   }
   ~TestSegmentConverter() {}
 
@@ -47,9 +47,9 @@ class TestSegmentConverter : public testing::Test
   std::size_t panel_1_num_led_ = 20;
   std::size_t panel_2_num_led_ = 30;
 
-  std::unique_ptr<panther_lights::SegmentConverter> segment_converter_;
-  std::unordered_map<std::string, std::shared_ptr<panther_lights::LEDSegment>> segments_;
-  std::unordered_map<std::size_t, std::shared_ptr<panther_lights::LEDPanel>> led_panels_;
+  std::unique_ptr<husarion_ugv_lights::SegmentConverter> segment_converter_;
+  std::unordered_map<std::string, std::shared_ptr<husarion_ugv_lights::LEDSegment>> segments_;
+  std::unordered_map<std::size_t, std::shared_ptr<husarion_ugv_lights::LEDPanel>> led_panels_;
 };
 
 YAML::Node TestSegmentConverter::CreateSegmentDescription(
@@ -63,17 +63,17 @@ YAML::Node TestSegmentConverter::CreateSegmentDescription(
 
 YAML::Node TestSegmentConverter::CreateImageAnimationDescription()
 {
-  return YAML::Load("{image: $(find panther_lights)/test/files/animation.png, duration: 2}");
+  return YAML::Load("{image: $(find husarion_ugv_lights)/test/files/animation.png, duration: 2}");
 }
 
 TEST_F(TestSegmentConverter, ConvertInvalidChannel)
 {
   segments_.emplace(
     "name",
-    std::make_shared<panther_lights::LEDSegment>(CreateSegmentDescription(0, 10, 123), 50.0));
+    std::make_shared<husarion_ugv_lights::LEDSegment>(CreateSegmentDescription(0, 10, 123), 50.0));
   const auto anim_desc = CreateImageAnimationDescription();
   ASSERT_NO_THROW(
-    segments_.at("name")->SetAnimation("panther_lights::ImageAnimation", anim_desc, false));
+    segments_.at("name")->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false));
 
   EXPECT_THROW(segment_converter_->Convert(segments_, led_panels_), std::out_of_range);
 }
@@ -81,11 +81,11 @@ TEST_F(TestSegmentConverter, ConvertInvalidChannel)
 TEST_F(TestSegmentConverter, ConvertInvalidLedRange)
 {
   segments_.emplace(
-    "name", std::make_shared<panther_lights::LEDSegment>(
+    "name", std::make_shared<husarion_ugv_lights::LEDSegment>(
               CreateSegmentDescription(panel_1_num_led_, panel_1_num_led_ + 1, 1), 50.0));
   const auto anim_desc = CreateImageAnimationDescription();
   ASSERT_NO_THROW(
-    segments_.at("name")->SetAnimation("panther_lights::ImageAnimation", anim_desc, false));
+    segments_.at("name")->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false));
 
   EXPECT_THROW(segment_converter_->Convert(segments_, led_panels_), std::runtime_error);
 }
@@ -93,17 +93,17 @@ TEST_F(TestSegmentConverter, ConvertInvalidLedRange)
 TEST_F(TestSegmentConverter, ConvertSingleSegmentForEachPanel)
 {
   segments_.emplace(
-    "name_1", std::make_shared<panther_lights::LEDSegment>(
+    "name_1", std::make_shared<husarion_ugv_lights::LEDSegment>(
                 CreateSegmentDescription(0, panel_1_num_led_ - 1, 1), 50.0));
   segments_.emplace(
-    "name_2", std::make_shared<panther_lights::LEDSegment>(
+    "name_2", std::make_shared<husarion_ugv_lights::LEDSegment>(
                 CreateSegmentDescription(0, panel_2_num_led_ - 1, 2), 50.0));
 
   const auto anim_desc = CreateImageAnimationDescription();
 
   for (auto & segment : segments_) {
     ASSERT_NO_THROW(
-      segment.second->SetAnimation("panther_lights::ImageAnimation", anim_desc, false));
+      segment.second->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false));
     ASSERT_NO_THROW(segment.second->UpdateAnimation());
   }
 
@@ -113,28 +113,28 @@ TEST_F(TestSegmentConverter, ConvertSingleSegmentForEachPanel)
 TEST_F(TestSegmentConverter, ConvertMultipleSegments)
 {
   segments_.emplace(
-    "name_1", std::make_shared<panther_lights::LEDSegment>(
+    "name_1", std::make_shared<husarion_ugv_lights::LEDSegment>(
                 CreateSegmentDescription(0, std::size_t(panel_1_num_led_ / 2) - 1, 1), 50.0));
   segments_.emplace(
     "name_2",
-    std::make_shared<panther_lights::LEDSegment>(
+    std::make_shared<husarion_ugv_lights::LEDSegment>(
       CreateSegmentDescription(std::size_t(panel_1_num_led_ / 2), panel_1_num_led_ - 1, 1), 50.0));
 
   segments_.emplace(
-    "name_3", std::make_shared<panther_lights::LEDSegment>(
+    "name_3", std::make_shared<husarion_ugv_lights::LEDSegment>(
                 CreateSegmentDescription(0, (panel_2_num_led_ / 4) - 1, 2), 50.0));
   segments_.emplace(
     "name_4",
-    std::make_shared<panther_lights::LEDSegment>(
+    std::make_shared<husarion_ugv_lights::LEDSegment>(
       CreateSegmentDescription((panel_2_num_led_ / 4), (panel_2_num_led_ / 2) - 1, 2), 50.0));
   segments_.emplace(
-    "name_5", std::make_shared<panther_lights::LEDSegment>(
+    "name_5", std::make_shared<husarion_ugv_lights::LEDSegment>(
                 CreateSegmentDescription((panel_2_num_led_ / 2), panel_2_num_led_ - 1, 2), 50.0));
 
   const auto anim_desc = CreateImageAnimationDescription();
   for (auto & segment : segments_) {
     ASSERT_NO_THROW(
-      segment.second->SetAnimation("panther_lights::ImageAnimation", anim_desc, false));
+      segment.second->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false));
     ASSERT_NO_THROW(segment.second->UpdateAnimation());
   }
 
@@ -150,11 +150,11 @@ TEST_F(TestSegmentConverter, ConvertBrightnessOverride)
   anim_desc["brightness"] = float_brightness;
 
   segments_.emplace(
-    "name", std::make_shared<panther_lights::LEDSegment>(
+    "name", std::make_shared<husarion_ugv_lights::LEDSegment>(
               CreateSegmentDescription(0, panel_1_num_led_ - 1, channel), 50.0));
 
   ASSERT_NO_THROW(
-    segments_.at("name")->SetAnimation("panther_lights::ImageAnimation", anim_desc, false));
+    segments_.at("name")->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false));
 
   segment_converter_->Convert(segments_, led_panels_);
   ASSERT_NO_THROW(segment_converter_->Convert(segments_, led_panels_));
@@ -169,10 +169,10 @@ TEST_F(TestSegmentConverter, ConvertBrightnessOverride)
 TEST_F(TestSegmentConverter, ConvertNoThrowIfAnimationNotSet)
 {
   segments_.emplace(
-    "name_1", std::make_shared<panther_lights::LEDSegment>(
+    "name_1", std::make_shared<husarion_ugv_lights::LEDSegment>(
                 CreateSegmentDescription(0, panel_1_num_led_ - 1, 1), 50.0));
   segments_.emplace(
-    "name_2", std::make_shared<panther_lights::LEDSegment>(
+    "name_2", std::make_shared<husarion_ugv_lights::LEDSegment>(
                 CreateSegmentDescription(0, panel_2_num_led_ - 1, 2), 50.0));
 
   EXPECT_NO_THROW(segment_converter_->Convert(segments_, led_panels_));
diff --git a/panther_lights/test/unit/test_apa102.cpp b/husarion_ugv_lights/test/unit/test_apa102.cpp
similarity index 97%
rename from panther_lights/test/unit/test_apa102.cpp
rename to husarion_ugv_lights/test/unit/test_apa102.cpp
index 2f5bbd2f..2d43684c 100644
--- a/panther_lights/test/unit/test_apa102.cpp
+++ b/husarion_ugv_lights/test/unit/test_apa102.cpp
@@ -15,13 +15,13 @@
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
 
-#include "panther_lights/apa102.hpp"
+#include "husarion_ugv_lights/apa102.hpp"
 
 static constexpr char kMockDeviceName[] = "/dev/mocked_device";
 static constexpr int kStartFrame = 0x00;
 static constexpr int kEndFrame = 0xFF;
 
-class MockSPIDevice : public panther_lights::SPIDeviceInterface
+class MockSPIDevice : public husarion_ugv_lights::SPIDeviceInterface
 {
 public:
   MOCK_METHOD(int, Open, (const std::string & device), (override));
@@ -32,7 +32,7 @@ class MockSPIDevice : public panther_lights::SPIDeviceInterface
   using NiceMock = testing::NiceMock<MockSPIDevice>;
 };
 
-class APA102Wrapper : public panther_lights::APA102
+class APA102Wrapper : public husarion_ugv_lights::APA102
 {
 public:
   APA102Wrapper(std::shared_ptr<MockSPIDevice> spi_device, const std::string & device_name)
diff --git a/panther_lights/test/unit/test_lights_controller_node.cpp b/husarion_ugv_lights/test/unit/test_lights_controller_node.cpp
similarity index 87%
rename from panther_lights/test/unit/test_lights_controller_node.cpp
rename to husarion_ugv_lights/test/unit/test_lights_controller_node.cpp
index 99b0a069..07546ac4 100644
--- a/panther_lights/test/unit/test_lights_controller_node.cpp
+++ b/husarion_ugv_lights/test/unit/test_lights_controller_node.cpp
@@ -26,10 +26,10 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_lights/lights_controller_node.hpp"
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_lights/lights_controller_node.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
-class ControllerNodeWrapper : public panther_lights::LightsControllerNode
+class ControllerNodeWrapper : public husarion_ugv_lights::LightsControllerNode
 {
 public:
   ControllerNodeWrapper(const rclcpp::NodeOptions & options) : LightsControllerNode(options) {}
@@ -55,9 +55,12 @@ class ControllerNodeWrapper : public panther_lights::LightsControllerNode
     return LightsControllerNode::AddAnimationToQueue(animation_id, repeating, param);
   }
 
-  std::shared_ptr<panther_lights::LEDAnimationsQueue> GetQueue() { return this->animations_queue_; }
+  std::shared_ptr<husarion_ugv_lights::LEDAnimationsQueue> GetQueue()
+  {
+    return this->animations_queue_;
+  }
 
-  std::shared_ptr<panther_lights::LEDAnimation> GetCurrentAnimation()
+  std::shared_ptr<husarion_ugv_lights::LEDAnimation> GetCurrentAnimation()
   {
     return this->current_animation_;
   }
@@ -116,11 +119,11 @@ void TestLightsControllerNode::CreateLEDConfig(const std::filesystem::path file_
   segments_map["test"] = std::vector<std::string>(1, kTestSegmentName);
 
   YAML::Node animation;
-  animation["image"] = "$(find panther_lights)/test/files/strip01_red.png";
+  animation["image"] = "$(find husarion_ugv_lights)/test/files/strip01_red.png";
   animation["duration"] = 2;
 
   YAML::Node animation_desc;
-  animation_desc["type"] = "panther_lights::ImageAnimation";
+  animation_desc["type"] = "husarion_ugv_lights::ImageAnimation";
   animation_desc["segments"] = "test";
   animation_desc["animation"] = animation;
 
@@ -171,7 +174,7 @@ TEST_F(TestLightsControllerNode, InitializeLEDPanelsThrowRepeatingChannel)
   YAML::Node panels_desc;
   panels_desc["panels"] = panels;
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { lights_controller_node_->InitializeLEDPanels(panels_desc["panels"]); },
     "Multiple panels with channel nr"));
 }
@@ -195,7 +198,7 @@ TEST_F(TestLightsControllerNode, InitializeLEDSegmentsThrowRepeatingName)
   YAML::Node segments_desc;
   segments_desc["segments"] = segments;
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { lights_controller_node_->InitializeLEDSegments(segments_desc["segments"], 50.0); },
     "Multiple segments with given name found"));
 }
@@ -206,13 +209,13 @@ TEST_F(TestLightsControllerNode, LoadAnimationInvalidPriority)
   led_animation_desc["id"] = 11;
   led_animation_desc["priority"] = 0;
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { lights_controller_node_->LoadAnimation(led_animation_desc); },
     "Invalid LED animation priority"));
 
   led_animation_desc["priority"] = 4;
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { lights_controller_node_->LoadAnimation(led_animation_desc); },
     "Invalid LED animation priority"));
 }
@@ -225,14 +228,14 @@ TEST_F(TestLightsControllerNode, LoadAnimationThrowRepeatingID)
 
   ASSERT_NO_THROW(lights_controller_node_->LoadAnimation(led_animation_desc));
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { lights_controller_node_->LoadAnimation(led_animation_desc); },
     "Animation with given ID already exists"));
 }
 
 TEST_F(TestLightsControllerNode, AddAnimationToQueueThrowBadAnimationID)
 {
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { lights_controller_node_->AddAnimationToQueue(99, false); }, "No animation with ID:"));
 }
 
diff --git a/panther_lights/test/unit/test_lights_driver_node.cpp b/husarion_ugv_lights/test/unit/test_lights_driver_node.cpp
similarity index 95%
rename from panther_lights/test/unit/test_lights_driver_node.cpp
rename to husarion_ugv_lights/test/unit/test_lights_driver_node.cpp
index 09918acf..91fcc0c1 100644
--- a/panther_lights/test/unit/test_lights_driver_node.cpp
+++ b/husarion_ugv_lights/test/unit/test_lights_driver_node.cpp
@@ -25,15 +25,15 @@
 
 #include "panther_msgs/srv/set_led_brightness.hpp"
 
-#include "panther_lights/apa102.hpp"
-#include "panther_lights/lights_driver_node.hpp"
-#include "panther_utils/test/ros_test_utils.hpp"
+#include "husarion_ugv_lights/apa102.hpp"
+#include "husarion_ugv_lights/lights_driver_node.hpp"
+#include "husarion_ugv_utils/test/ros_test_utils.hpp"
 
 using ImageMsg = sensor_msgs::msg::Image;
 using SetBoolSrv = std_srvs::srv::SetBool;
 using SetLEDBrightnessSrv = panther_msgs::srv::SetLEDBrightness;
 
-class MockAPA102 : public panther_lights::APA102Interface
+class MockAPA102 : public husarion_ugv_lights::APA102Interface
 {
 public:
   MOCK_METHOD(void, SetGlobalBrightness, (const std::uint8_t brightness), (override));
@@ -44,7 +44,7 @@ class MockAPA102 : public panther_lights::APA102Interface
 };
 
 // LightsDriverNode constructor implemented for testing purposes
-panther_lights::LightsDriverNode::LightsDriverNode(
+husarion_ugv_lights::LightsDriverNode::LightsDriverNode(
   APA102Interface::SharedPtr channel_1, APA102Interface::SharedPtr channel_2,
   const rclcpp::NodeOptions & options)
 : Node("lights_driver", options),
@@ -60,7 +60,7 @@ panther_lights::LightsDriverNode::LightsDriverNode(
   frame_timeout_ = 0.1;
 };
 
-class DriverNodeWrapper : public panther_lights::LightsDriverNode
+class DriverNodeWrapper : public husarion_ugv_lights::LightsDriverNode
 {
 public:
   DriverNodeWrapper(
@@ -78,7 +78,7 @@ class DriverNodeWrapper : public panther_lights::LightsDriverNode
   }
 
   void FrameCB(
-    const ImageMsg::UniquePtr & msg, const panther_lights::APA102Interface::SharedPtr & panel,
+    const ImageMsg::UniquePtr & msg, const husarion_ugv_lights::APA102Interface::SharedPtr & panel,
     const rclcpp::Time & last_time, const std::string & panel_name)
   {
     return LightsDriverNode::FrameCB(msg, panel, last_time, panel_name);
diff --git a/panther_localization/CHANGELOG.rst b/husarion_ugv_localization/CHANGELOG.rst
similarity index 100%
rename from panther_localization/CHANGELOG.rst
rename to husarion_ugv_localization/CHANGELOG.rst
diff --git a/panther_controller/CMakeLists.txt b/husarion_ugv_localization/CMakeLists.txt
similarity index 82%
rename from panther_controller/CMakeLists.txt
rename to husarion_ugv_localization/CMakeLists.txt
index c6489681..2ce65ec9 100644
--- a/panther_controller/CMakeLists.txt
+++ b/husarion_ugv_localization/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10.2)
-project(panther_controller)
+project(husarion_ugv_localization)
 
 find_package(ament_cmake REQUIRED)
 
diff --git a/panther_localization/README.md b/husarion_ugv_localization/README.md
similarity index 98%
rename from panther_localization/README.md
rename to husarion_ugv_localization/README.md
index 6dc52763..54eacef5 100644
--- a/panther_localization/README.md
+++ b/husarion_ugv_localization/README.md
@@ -1,4 +1,4 @@
-# panther_localization
+# husarion_ugv_localization
 
 The package is responsible for activating mods responsible for fusion of data related to the robot's location.
 
diff --git a/panther_localization/config/enu_localization.yaml b/husarion_ugv_localization/config/enu_localization.yaml
similarity index 99%
rename from panther_localization/config/enu_localization.yaml
rename to husarion_ugv_localization/config/enu_localization.yaml
index afaee4e2..6b2668cc 100644
--- a/panther_localization/config/enu_localization.yaml
+++ b/husarion_ugv_localization/config/enu_localization.yaml
@@ -41,7 +41,7 @@
       use_control: true
       control_timeout: 0.5
       control_config: [true, true, false, false, false, true]
-      acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside panther_controller/config
+      acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside husarion_ugv_controller/config
 
       predict_to_current_time: true
 
diff --git a/panther_localization/config/enu_localization_with_gps.yaml b/husarion_ugv_localization/config/enu_localization_with_gps.yaml
similarity index 99%
rename from panther_localization/config/enu_localization_with_gps.yaml
rename to husarion_ugv_localization/config/enu_localization_with_gps.yaml
index 96c60320..ef1fa6ff 100644
--- a/panther_localization/config/enu_localization_with_gps.yaml
+++ b/husarion_ugv_localization/config/enu_localization_with_gps.yaml
@@ -51,7 +51,7 @@
       use_control: true
       control_timeout: 0.5
       control_config: [true, true, false, false, false, true]
-      acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside panther_controller/config
+      acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside husarion_ugv_controller/config
 
       predict_to_current_time: true
 
diff --git a/panther_localization/config/nmea_navsat.yaml b/husarion_ugv_localization/config/nmea_navsat.yaml
similarity index 100%
rename from panther_localization/config/nmea_navsat.yaml
rename to husarion_ugv_localization/config/nmea_navsat.yaml
diff --git a/panther_localization/config/relative_localization.yaml b/husarion_ugv_localization/config/relative_localization.yaml
similarity index 99%
rename from panther_localization/config/relative_localization.yaml
rename to husarion_ugv_localization/config/relative_localization.yaml
index 3e29fcdd..5f420735 100644
--- a/panther_localization/config/relative_localization.yaml
+++ b/husarion_ugv_localization/config/relative_localization.yaml
@@ -41,7 +41,7 @@
       use_control: true
       control_timeout: 0.5
       control_config: [true, true, false, false, false, true]
-      acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside panther_controller/config
+      acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside husarion_ugv_controller/config
 
       predict_to_current_time: true
 
diff --git a/panther_localization/config/relative_localization_with_gps.yaml b/husarion_ugv_localization/config/relative_localization_with_gps.yaml
similarity index 99%
rename from panther_localization/config/relative_localization_with_gps.yaml
rename to husarion_ugv_localization/config/relative_localization_with_gps.yaml
index 0a98abcb..fa842675 100644
--- a/panther_localization/config/relative_localization_with_gps.yaml
+++ b/husarion_ugv_localization/config/relative_localization_with_gps.yaml
@@ -51,7 +51,7 @@
       use_control: true
       control_timeout: 0.5
       control_config: [true, true, false, false, false, true]
-      acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside panther_controller/config
+      acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside husarion_ugv_controller/config
 
       predict_to_current_time: true
 
diff --git a/panther_localization/launch/localization.launch.py b/husarion_ugv_localization/launch/localization.launch.py
similarity index 96%
rename from panther_localization/launch/localization.launch.py
rename to husarion_ugv_localization/launch/localization.launch.py
index 5447af64..e948eb28 100644
--- a/panther_localization/launch/localization.launch.py
+++ b/husarion_ugv_localization/launch/localization.launch.py
@@ -90,7 +90,7 @@ def generate_launch_description():
     declare_localization_config_path_arg = DeclareLaunchArgument(
         "localization_config_path",
         default_value=PathJoinSubstitution(
-            [FindPackageShare("panther_localization"), "config", localization_config_filename]
+            [FindPackageShare("husarion_ugv_localization"), "config", localization_config_filename]
         ),
         description="Specify the path to the localization configuration file.",
     )
@@ -113,7 +113,7 @@ def generate_launch_description():
     nmea_navsat_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare("panther_localization"), "launch", "nmea_navsat.launch.py"]
+                [FindPackageShare("husarion_ugv_localization"), "launch", "nmea_navsat.launch.py"]
             )
         ),
         launch_arguments={"namespace": namespace}.items(),
diff --git a/panther_localization/launch/nmea_navsat.launch.py b/husarion_ugv_localization/launch/nmea_navsat.launch.py
similarity index 96%
rename from panther_localization/launch/nmea_navsat.launch.py
rename to husarion_ugv_localization/launch/nmea_navsat.launch.py
index b121eb9d..70fe996d 100644
--- a/panther_localization/launch/nmea_navsat.launch.py
+++ b/husarion_ugv_localization/launch/nmea_navsat.launch.py
@@ -31,7 +31,7 @@ def generate_launch_description():
     declare_nmea_params_path_arg = DeclareLaunchArgument(
         "nmea_params_path",
         default_value=PathJoinSubstitution(
-            [FindPackageShare("panther_localization"), "config", "nmea_navsat.yaml"]
+            [FindPackageShare("husarion_ugv_localization"), "config", "nmea_navsat.yaml"]
         ),
         description="Path to the parameter file for the nmea_socket_driver node.",
     )
diff --git a/panther_localization/package.xml b/husarion_ugv_localization/package.xml
similarity index 88%
rename from panther_localization/package.xml
rename to husarion_ugv_localization/package.xml
index 7cef452a..bc7cd0d5 100644
--- a/panther_localization/package.xml
+++ b/husarion_ugv_localization/package.xml
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>panther_localization</name>
+  <name>husarion_ugv_localization</name>
   <version>2.1.1</version>
-  <description>robot localization configuration for Panther</description>
+  <description>robot localization configuration for Husarion UGV</description>
   <maintainer email="support@husarion.com">Husarion</maintainer>
   <license>Apache License 2.0</license>
 
diff --git a/panther_manager/CHANGELOG.rst b/husarion_ugv_manager/CHANGELOG.rst
similarity index 100%
rename from panther_manager/CHANGELOG.rst
rename to husarion_ugv_manager/CHANGELOG.rst
diff --git a/panther_manager/CMakeLists.txt b/husarion_ugv_manager/CMakeLists.txt
similarity index 95%
rename from panther_manager/CMakeLists.txt
rename to husarion_ugv_manager/CMakeLists.txt
index fbf7fc92..29286611 100644
--- a/panther_manager/CMakeLists.txt
+++ b/husarion_ugv_manager/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(panther_manager)
+project(husarion_ugv_manager)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
@@ -12,7 +12,7 @@ set(PACKAGE_DEPENDENCIES
     behaviortree_ros2
     libssh
     panther_msgs
-    panther_utils
+    husarion_ugv_utils
     rclcpp
     rclcpp_action
     sensor_msgs
@@ -67,7 +67,7 @@ ament_target_dependencies(
   safety_manager_node
   behaviortree_ros2
   panther_msgs
-  panther_utils
+  husarion_ugv_utils
   rclcpp
   sensor_msgs
   std_msgs)
@@ -80,7 +80,7 @@ ament_target_dependencies(
   lights_manager_node
   behaviortree_ros2
   panther_msgs
-  panther_utils
+  husarion_ugv_utils
   rclcpp
   sensor_msgs
   std_msgs)
@@ -167,8 +167,9 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_behavior_tree_utils
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
            $<INSTALL_INTERFACE:include>)
-  ament_target_dependencies(${PROJECT_NAME}_test_behavior_tree_utils
-                            behaviortree_cpp behaviortree_ros2 panther_utils)
+  ament_target_dependencies(
+    ${PROJECT_NAME}_test_behavior_tree_utils behaviortree_cpp behaviortree_ros2
+    husarion_ugv_utils)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_behavior_tree_manager
@@ -178,7 +179,7 @@ if(BUILD_TESTING)
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_behavior_tree_manager
-                            behaviortree_cpp panther_utils)
+                            behaviortree_cpp husarion_ugv_utils)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_lights_manager_node test/test_lights_manager_node.cpp
@@ -192,7 +193,7 @@ if(BUILD_TESTING)
     behaviortree_cpp
     behaviortree_ros2
     panther_msgs
-    panther_utils
+    husarion_ugv_utils
     rclcpp
     sensor_msgs
     std_msgs)
@@ -210,7 +211,7 @@ if(BUILD_TESTING)
     behaviortree_cpp
     behaviortree_ros2
     panther_msgs
-    panther_utils
+    husarion_ugv_utils
     rclcpp
     sensor_msgs
     std_msgs)
@@ -227,7 +228,7 @@ if(BUILD_TESTING)
     behaviortree_cpp
     behaviortree_ros2
     panther_msgs
-    panther_utils
+    husarion_ugv_utils
     rclcpp
     sensor_msgs
     std_msgs)
@@ -245,7 +246,7 @@ if(BUILD_TESTING)
     behaviortree_cpp
     behaviortree_ros2
     panther_msgs
-    panther_utils
+    husarion_ugv_utils
     rclcpp
     sensor_msgs
     std_msgs
diff --git a/panther_manager/CONFIGURATION.md b/husarion_ugv_manager/CONFIGURATION.md
similarity index 96%
rename from panther_manager/CONFIGURATION.md
rename to husarion_ugv_manager/CONFIGURATION.md
index 6454c886..adc2337a 100644
--- a/panther_manager/CONFIGURATION.md
+++ b/husarion_ugv_manager/CONFIGURATION.md
@@ -1,4 +1,4 @@
-# panther_manager
+# husarion_ugv_manager
 
 ## Shutdown Behavior
 
@@ -39,7 +39,7 @@ ssh-copy-id username@10.15.20.XX
 
 ## Faults Handle
 
-After receiving a message on the `battery/battery_status` topic, the `panther_manager` node makes decisions regarding safety measures. For more information regarding the power supply status, please refer to the [BatteryState](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) message definition and [adc_battery.cpp](../panther_battery/src/battery/adc_battery.cpp) implementation.
+After receiving a message on the `battery/battery_status` topic, the `husarion_ugv_manager` node makes decisions regarding safety measures. For more information regarding the power supply status, please refer to the [BatteryState](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) message definition and [adc_battery.cpp](../husarion_ugv_battery/src/battery/adc_battery.cpp) implementation.
 
 | Power Supply Health | Procedure                                                                                                                                                                                                                     |
 | ------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
@@ -204,7 +204,7 @@ To use your customized project, you can modify the `bt_project_file` ROS paramet
 
 ### Real-time Visualization
 
-Groot2 also provides a real-time visualization tool that allows you to see and debug actively running trees. To use this tool with trees launched with the `panther_manager` package, you need to specify the port associated with the tree you want to visualize. The ports for each tree are listed below:
+Groot2 also provides a real-time visualization tool that allows you to see and debug actively running trees. To use this tool with trees launched with the `husarion_ugv_manager` package, you need to specify the port associated with the tree you want to visualize. The ports for each tree are listed below:
 
 - Lights tree: `10.15.20.2:5555`
 - Safety tree: `10.15.20.2:6666`
diff --git a/panther_manager/README.md b/husarion_ugv_manager/README.md
similarity index 92%
rename from panther_manager/README.md
rename to husarion_ugv_manager/README.md
index 718abece..b8a9795f 100644
--- a/panther_manager/README.md
+++ b/husarion_ugv_manager/README.md
@@ -1,6 +1,6 @@
-# panther_manager
+# husarion_ugv_manager
 
-A package containing nodes responsible for high-level control of Husarion Panther robot.
+A package containing nodes responsible for high-level control of Husarion UGV.
 
 ## Launch Files
 
@@ -11,8 +11,8 @@ This package contains:
 ## Configuration Files
 
 - [`lights.xml`](./behavior_trees/lights.xml): BehaviorTree for managing lights.
-- [`PantherLightsBT.btproj`](./behavior_trees/PantherLightsBT.btproj): BehaviorTree project for managing Panther lights.
-- [`PantherSafetyBT.btproj`](./behavior_trees/PantherSafetyBT.btproj): BehaviorTree project for managing Panther safety protocols.
+- [`LightsBT.btproj`](./behavior_trees/LightsBT.btproj): BehaviorTree project for managing Panther lights.
+- [`SafetyBT.btproj`](./behavior_trees/SafetyBT.btproj): BehaviorTree project for managing Panther safety protocols.
 - [`safety.xml`](./behavior_trees/safety.xml): BehaviorTree for monitoring and managing dangerous situations.
 - [`shutdown.xml`](./behavior_trees/shutdown.xml): BehaviorTree for initiating shutdown procedures.
 - [`lights_manager.yaml`](./config/lights_manager.yaml): Contains parameters for the `lights_manager` node.
@@ -42,7 +42,7 @@ Node responsible for managing Bumper Lights animation scheduling.
 - `battery.percent.threshold.critical` [*float*, default: **0.1**]: If the Battery percentage drops below this value, an animation indicating a critical Battery state will start being displayed.
 - `battery.percent.threshold.low` [*float*, default: **0.4**]: If the Battery percentage drops below this value, the animation indicating a low Battery state will start being displayed.
 - `battery.percent.window_len` [*int*, default: **6**]: Moving average window length used to smooth out Battery percentage readings.
-- `bt_project_path` [*string*, default: **$(find panther_manager)/config/PantherBT.btproj**]: Path to a BehaviorTree project.
+- `bt_project_path` [*string*, default: **$(find husarion_ugv_manager)/config/PantherBT.btproj**]: Path to a BehaviorTree project.
 - `plugin_libs` [*list*, default: **Empty list**]: List with names of plugins that are used in the BT project.
 - `ros_communication_timeout.availability` [*float*, default: **1.0**]: Timeout **[s]** to wait for a service/action while initializing BT node.
 - `ros_communication_timeout.response` [*float*, default: **1.0**]: Timeout **[s]** to receive a service/action response after call.
@@ -70,7 +70,7 @@ Node responsible for managing safety features, and software shutdown of componen
 #### Parameters
 
 - `battery.temp.window_len` [*int*, default: **6**]: Moving average window length used to smooth out temperature readings of the Battery.
-- `bt_project_path` [*string*, default: **$(find panther_manager)/config/PantherBT.btproj**]: Path to a BehaviorTree project.
+- `bt_project_path` [*string*, default: **$(find husarion_ugv_manager)/config/PantherBT.btproj**]: Path to a BehaviorTree project.
 - `cpu.temp.fan_off` [*float*, default: **60.0**]: Temperature in **[&deg;C]** of the Built-in Computer's CPU, below which the fan is turned off.
 - `cpu.temp.fan_on` [*float*, default: **70.0**]: Temperature in **[&deg;C]** of the Built-in Computer's CPU, above which the fan is turned on.
 - `cpu.temp.window_len` [*int*, default: **6**]: Moving average window length used to smooth out temperature readings of the Built-in Computer's CPU.
diff --git a/panther_manager/behavior_trees/PantherLightsBT.btproj b/husarion_ugv_manager/behavior_trees/LightsBT.btproj
similarity index 93%
rename from panther_manager/behavior_trees/PantherLightsBT.btproj
rename to husarion_ugv_manager/behavior_trees/LightsBT.btproj
index ab0f59aa..61bba3ef 100644
--- a/panther_manager/behavior_trees/PantherLightsBT.btproj
+++ b/husarion_ugv_manager/behavior_trees/LightsBT.btproj
@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<root BTCPP_format="4" project_name="PantherLightsBT">
+<root BTCPP_format="4" project_name="LightsBT">
     <include path="lights.xml"/>
     <!-- Description of Node Models (used by Groot) -->
     <TreeNodesModel>
diff --git a/panther_manager/behavior_trees/PantherSafetyBT.btproj b/husarion_ugv_manager/behavior_trees/SafetyBT.btproj
similarity index 97%
rename from panther_manager/behavior_trees/PantherSafetyBT.btproj
rename to husarion_ugv_manager/behavior_trees/SafetyBT.btproj
index 4ff1836e..b7f2bbc5 100644
--- a/panther_manager/behavior_trees/PantherSafetyBT.btproj
+++ b/husarion_ugv_manager/behavior_trees/SafetyBT.btproj
@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<root BTCPP_format="4" project_name="PantherSafetyBT">
+<root BTCPP_format="4" project_name="SafetyBT">
     <include path="safety.xml"/>
     <include path="shutdown.xml"/>
     <!-- Description of Node Models (used by Groot) -->
diff --git a/panther_manager/behavior_trees/lights.xml b/husarion_ugv_manager/behavior_trees/lights.xml
similarity index 100%
rename from panther_manager/behavior_trees/lights.xml
rename to husarion_ugv_manager/behavior_trees/lights.xml
diff --git a/panther_manager/behavior_trees/safety.xml b/husarion_ugv_manager/behavior_trees/safety.xml
similarity index 100%
rename from panther_manager/behavior_trees/safety.xml
rename to husarion_ugv_manager/behavior_trees/safety.xml
diff --git a/panther_manager/behavior_trees/shutdown.xml b/husarion_ugv_manager/behavior_trees/shutdown.xml
similarity index 100%
rename from panther_manager/behavior_trees/shutdown.xml
rename to husarion_ugv_manager/behavior_trees/shutdown.xml
diff --git a/panther_manager/config/lights_manager.yaml b/husarion_ugv_manager/config/lights_manager.yaml
similarity index 100%
rename from panther_manager/config/lights_manager.yaml
rename to husarion_ugv_manager/config/lights_manager.yaml
diff --git a/panther_manager/config/safety_manager.yaml b/husarion_ugv_manager/config/safety_manager.yaml
similarity index 100%
rename from panther_manager/config/safety_manager.yaml
rename to husarion_ugv_manager/config/safety_manager.yaml
diff --git a/panther_manager/config/shutdown_hosts.yaml b/husarion_ugv_manager/config/shutdown_hosts.yaml
similarity index 100%
rename from panther_manager/config/shutdown_hosts.yaml
rename to husarion_ugv_manager/config/shutdown_hosts.yaml
diff --git a/panther_manager/include/panther_manager/behavior_tree_manager.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_manager.hpp
similarity index 92%
rename from panther_manager/include/panther_manager/behavior_tree_manager.hpp
rename to husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_manager.hpp
index 24d6eed0..831a0c7b 100644
--- a/panther_manager/include/panther_manager/behavior_tree_manager.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_manager.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_BEHAVIOR_TREE_MANAGER_HPP_
-#define PANTHER_MANAGER_BEHAVIOR_TREE_MANAGER_HPP_
+#ifndef HUSARION_UGV_MANAGER_BEHAVIOR_TREE_MANAGER_HPP_
+#define HUSARION_UGV_MANAGER_BEHAVIOR_TREE_MANAGER_HPP_
 
 #include <any>
 #include <map>
@@ -24,7 +24,7 @@
 #include "behaviortree_cpp/loggers/groot2_publisher.h"
 #include "rclcpp/rclcpp.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 /**
@@ -93,6 +93,6 @@ class BehaviorTreeManager
   std::unique_ptr<BT::Groot2Publisher> groot_publisher_;
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_BEHAVIOR_TREE_MANAGER_HPP_
+#endif  // HUSARION_UGV_MANAGER_BEHAVIOR_TREE_MANAGER_HPP_
diff --git a/panther_manager/include/panther_manager/behavior_tree_utils.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp
similarity index 89%
rename from panther_manager/include/panther_manager/behavior_tree_utils.hpp
rename to husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp
index 360e174e..8d411136 100644
--- a/panther_manager/include/panther_manager/behavior_tree_utils.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_BEHAVIOR_TREE_UTILS_HPP_
-#define PANTHER_MANAGER_BEHAVIOR_TREE_UTILS_HPP_
+#ifndef HUSARION_UGV_MANAGER_BEHAVIOR_TREE_UTILS_HPP_
+#define HUSARION_UGV_MANAGER_BEHAVIOR_TREE_UTILS_HPP_
 
 #include <any>
 #include <map>
@@ -26,7 +26,7 @@
 #include "behaviortree_cpp/utils/shared_library.h"
 #include "behaviortree_ros2/plugins.hpp"
 
-namespace panther_manager::behavior_tree_utils
+namespace husarion_ugv_manager::behavior_tree_utils
 {
 
 /**
@@ -73,9 +73,9 @@ inline void RegisterBehaviorTree(
   RegisterBehaviorTree(factory, bt_project_path, plugin_libs);
 }
 
-}  // namespace panther_manager::behavior_tree_utils
+}  // namespace husarion_ugv_manager::behavior_tree_utils
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 // TODO: @pawelirh move to a separate file with an appropriate abstraction layer
 /**
@@ -88,6 +88,6 @@ inline std::string GetLoggerPrefix(const std::string & bt_node_name)
 {
   return std::string("[" + bt_node_name + "] ");
 }
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_BEHAVIOR_TREE_UTILS_HPP_
+#endif  // HUSARION_UGV_MANAGER_BEHAVIOR_TREE_UTILS_HPP_
diff --git a/panther_manager/include/panther_manager/lights_manager_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp
similarity index 84%
rename from panther_manager/include/panther_manager/lights_manager_node.hpp
rename to husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp
index 21937cd8..6fd54433 100644
--- a/panther_manager/include/panther_manager/lights_manager_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_LIGHTS_MANAGER_NODE_HPP_
-#define PANTHER_MANAGER_LIGHTS_MANAGER_NODE_HPP_
+#ifndef HUSARION_UGV_MANAGER_LIGHTS_MANAGER_NODE_HPP_
+#define HUSARION_UGV_MANAGER_LIGHTS_MANAGER_NODE_HPP_
 
 #include <memory>
 #include <string>
@@ -26,11 +26,11 @@
 
 #include "panther_msgs/msg/led_animation.hpp"
 
-#include "panther_utils/moving_average.hpp"
+#include "husarion_ugv_utils/moving_average.hpp"
 
-#include <panther_manager/behavior_tree_manager.hpp>
+#include <husarion_ugv_manager/behavior_tree_manager.hpp>
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
@@ -77,10 +77,10 @@ class LightsManagerNode : public rclcpp::Node
   rclcpp::Subscription<BoolMsg>::SharedPtr e_stop_sub_;
   rclcpp::TimerBase::SharedPtr lights_tree_timer_;
 
-  std::unique_ptr<panther_utils::MovingAverage<double>> battery_percent_ma_;
+  std::unique_ptr<husarion_ugv_utils::MovingAverage<double>> battery_percent_ma_;
   BT::BehaviorTreeFactory factory_;
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_LIGHTS_MANAGER_NODE_HPP_
+#endif  // HUSARION_UGV_MANAGER_LIGHTS_MANAGER_NODE_HPP_
diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp
similarity index 81%
rename from panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp
rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp
index adafe1d4..f9bfae2e 100644
--- a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_BOOL_SERVICE_NODE_HPP_
-#define PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_BOOL_SERVICE_NODE_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_SET_BOOL_SERVICE_NODE_HPP_
+#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_SET_BOOL_SERVICE_NODE_HPP_
 
 #include <string>
 
@@ -22,7 +22,7 @@
 
 #include "std_srvs/srv/set_bool.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 class CallSetBoolService : public BT::RosServiceNode<std_srvs::srv::SetBool>
@@ -43,6 +43,6 @@ class CallSetBoolService : public BT::RosServiceNode<std_srvs::srv::SetBool>
   virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr & response) override;
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_BOOL_SERVICE_NODE_HPP_
+#endif  // HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_SET_BOOL_SERVICE_NODE_HPP_
diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp
similarity index 82%
rename from panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp
rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp
index 38a915a5..4fa2f8f8 100644
--- a/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_
-#define PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_
+#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_
 
 #include <string>
 
@@ -22,7 +22,7 @@
 
 #include "panther_msgs/srv/set_led_animation.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 class CallSetLedAnimationService : public BT::RosServiceNode<panther_msgs::srv::SetLEDAnimation>
@@ -47,6 +47,6 @@ class CallSetLedAnimationService : public BT::RosServiceNode<panther_msgs::srv::
   virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr & response) override;
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_
+#endif  // HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_
diff --git a/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_trigger_service_node.hpp
similarity index 81%
rename from panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp
rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_trigger_service_node.hpp
index 3e4e5551..2a9de666 100644
--- a/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_trigger_service_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_PLUGINS_ACTION_CALL_TRIGGER_SERVICE_NODE_HPP_
-#define PANTHER_MANAGER_PLUGINS_ACTION_CALL_TRIGGER_SERVICE_NODE_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_TRIGGER_SERVICE_NODE_HPP_
+#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_TRIGGER_SERVICE_NODE_HPP_
 
 #include <string>
 
@@ -22,7 +22,7 @@
 
 #include "std_srvs/srv/trigger.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 class CallTriggerService : public BT::RosServiceNode<std_srvs::srv::Trigger>
@@ -40,6 +40,6 @@ class CallTriggerService : public BT::RosServiceNode<std_srvs::srv::Trigger>
   virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr & response) override;
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_PLUGINS_ACTION_CALL_TRIGGER_SERVICE_NODE_HPP_
+#endif  // HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_TRIGGER_SERVICE_NODE_HPP_
diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp
similarity index 71%
rename from panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp
rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp
index 949565f6..10396a8c 100644
--- a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_
-#define PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_
+#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_
 
 #include <memory>
 #include <string>
@@ -22,11 +22,11 @@
 #include "behaviortree_cpp/basic_types.h"
 #include "yaml-cpp/yaml.h"
 
-#include "panther_manager/plugins/shutdown_host.hpp"
-#include "panther_manager/plugins/shutdown_hosts_node.hpp"
-#include "panther_utils/yaml_utils.hpp"
+#include "husarion_ugv_manager/plugins/shutdown_host.hpp"
+#include "husarion_ugv_manager/plugins/shutdown_hosts_node.hpp"
+#include "husarion_ugv_utils/yaml_utils.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 class ShutdownHostsFromFile : public ShutdownHosts
@@ -49,6 +49,6 @@ class ShutdownHostsFromFile : public ShutdownHosts
   bool UpdateHosts(std::vector<std::shared_ptr<ShutdownHost>> & hosts) override;
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_
+#endif  // HUSARION_UGV_MANAGER_PLUGINS_ACTION_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_
diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp
similarity index 78%
rename from panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp
rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp
index 630db650..be36a7f9 100644
--- a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_
-#define PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_
+#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_
 
 #include <memory>
 #include <string>
@@ -21,10 +21,10 @@
 
 #include "behaviortree_cpp/basic_types.h"
 
-#include "panther_manager/plugins/shutdown_host.hpp"
-#include "panther_manager/plugins/shutdown_hosts_node.hpp"
+#include "husarion_ugv_manager/plugins/shutdown_host.hpp"
+#include "husarion_ugv_manager/plugins/shutdown_hosts_node.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 class ShutdownSingleHost : public ShutdownHosts
@@ -52,6 +52,6 @@ class ShutdownSingleHost : public ShutdownHosts
   bool UpdateHosts(std::vector<std::shared_ptr<ShutdownHost>> & hosts) override;
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_
+#endif  // HUSARION_UGV_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_
diff --git a/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp
similarity index 79%
rename from panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp
rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp
index 7bc9709d..8e98387e 100644
--- a/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_
-#define PANTHER_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_
+#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_
 
 #include <string>
 
@@ -21,7 +21,7 @@
 #include "behaviortree_cpp/basic_types.h"
 #include "behaviortree_cpp/tree_node.h"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 class SignalShutdown : public BT::SyncActionNode
@@ -42,6 +42,6 @@ class SignalShutdown : public BT::SyncActionNode
   virtual BT::NodeStatus tick() override;
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_
+#endif  // HUSARION_UGV_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_
diff --git a/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp
similarity index 80%
rename from panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp
rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp
index 20a273e5..a453054f 100644
--- a/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_
-#define PANTHER_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_
+#define HUSARION_UGV_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_
 
 #include <chrono>
 #include <string>
@@ -22,7 +22,7 @@
 #include "behaviortree_cpp/decorator_node.h"
 #include "behaviortree_cpp/tree_node.h"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 class TickAfterTimeout : public BT::DecoratorNode
 {
@@ -40,6 +40,6 @@ class TickAfterTimeout : public BT::DecoratorNode
 
   BT::NodeStatus tick() override;
 };
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_
+#endif  // HUSARION_UGV_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_
diff --git a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/shutdown_host.hpp
similarity index 96%
rename from panther_manager/include/panther_manager/plugins/shutdown_host.hpp
rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/shutdown_host.hpp
index ce29367a..6f4729ba 100644
--- a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/shutdown_host.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_
-#define PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_
+#define HUSARION_UGV_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_
 
 #include <chrono>
 #include <cstdlib>
@@ -25,7 +25,7 @@
 
 #include "libssh/libsshpp.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 enum class ShutdownHostState {
@@ -268,6 +268,6 @@ class ShutdownHost
   }
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_
+#endif  // HUSARION_UGV_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_
diff --git a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/shutdown_hosts_node.hpp
similarity index 93%
rename from panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp
rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/shutdown_hosts_node.hpp
index 004c94a4..5c7b11ad 100644
--- a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/shutdown_hosts_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_
-#define PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_
+#define HUSARION_UGV_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_
 
 #include <algorithm>
 #include <memory>
@@ -29,11 +29,11 @@
 #include "behaviortree_cpp/tree_node.h"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_manager/plugins/shutdown_host.hpp"
+#include "husarion_ugv_manager/plugins/shutdown_host.hpp"
 
-#include "panther_manager/behavior_tree_utils.hpp"
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 class ShutdownHosts : public BT::StatefulActionNode
@@ -177,6 +177,6 @@ class ShutdownHosts : public BT::StatefulActionNode
   }
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_
+#endif  // HUSARION_UGV_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_
diff --git a/panther_manager/include/panther_manager/safety_manager_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp
similarity index 84%
rename from panther_manager/include/panther_manager/safety_manager_node.hpp
rename to husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp
index cde19e96..51dcb3e2 100644
--- a/panther_manager/include/panther_manager/safety_manager_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_SAFETY_MANAGER_NODE_HPP_
-#define PANTHER_MANAGER_SAFETY_MANAGER_NODE_HPP_
+#ifndef HUSARION_UGV_MANAGER_SAFETY_MANAGER_NODE_HPP_
+#define HUSARION_UGV_MANAGER_SAFETY_MANAGER_NODE_HPP_
 
 #include <map>
 #include <memory>
@@ -29,11 +29,11 @@
 #include "panther_msgs/msg/robot_driver_state.hpp"
 #include "panther_msgs/msg/system_status.hpp"
 
-#include "panther_utils/moving_average.hpp"
+#include "husarion_ugv_utils/moving_average.hpp"
 
-#include <panther_manager/behavior_tree_manager.hpp>
+#include <husarion_ugv_manager/behavior_tree_manager.hpp>
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
@@ -96,12 +96,12 @@ class SafetyManagerNode : public rclcpp::Node
   rclcpp::Subscription<SystemStatusMsg>::SharedPtr system_status_sub_;
   rclcpp::TimerBase::SharedPtr safety_tree_timer_;
 
-  std::unique_ptr<panther_utils::MovingAverage<double>> battery_temp_ma_;
-  std::unique_ptr<panther_utils::MovingAverage<double>> cpu_temp_ma_;
+  std::unique_ptr<husarion_ugv_utils::MovingAverage<double>> battery_temp_ma_;
+  std::unique_ptr<husarion_ugv_utils::MovingAverage<double>> cpu_temp_ma_;
 
-  std::map<std::string, std::unique_ptr<panther_utils::MovingAverage<double>>> driver_temp_ma_;
+  std::map<std::string, std::unique_ptr<husarion_ugv_utils::MovingAverage<double>>> driver_temp_ma_;
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_SAFETY_MANAGER_NODE_HPP_
+#endif  // HUSARION_UGV_MANAGER_SAFETY_MANAGER_NODE_HPP_
diff --git a/panther_manager/launch/manager.launch.py b/husarion_ugv_manager/launch/manager.launch.py
similarity index 86%
rename from panther_manager/launch/manager.launch.py
rename to husarion_ugv_manager/launch/manager.launch.py
index 0aaf5385..1412e924 100644
--- a/panther_manager/launch/manager.launch.py
+++ b/husarion_ugv_manager/launch/manager.launch.py
@@ -28,13 +28,13 @@
 
 def generate_launch_description():
 
-    panther_manager_dir = FindPackageShare("panther_manager")
+    husarion_ugv_manager_dir = FindPackageShare("husarion_ugv_manager")
 
     lights_bt_project_path = LaunchConfiguration("lights_bt_project_path")
     declare_lights_bt_project_path_arg = DeclareLaunchArgument(
         "lights_bt_project_path",
         default_value=PathJoinSubstitution(
-            [panther_manager_dir, "behavior_trees", "PantherLightsBT.btproj"]
+            [husarion_ugv_manager_dir, "behavior_trees", "LightsBT.btproj"]
         ),
         description="Path to BehaviorTree project file, responsible for lights management.",
     )
@@ -50,7 +50,7 @@ def generate_launch_description():
     declare_safety_bt_project_path_arg = DeclareLaunchArgument(
         "safety_bt_project_path",
         default_value=PathJoinSubstitution(
-            [panther_manager_dir, "behavior_trees", "PantherSafetyBT.btproj"]
+            [husarion_ugv_manager_dir, "behavior_trees", "SafetyBT.btproj"]
         ),
         description="Path to BehaviorTree project file, responsible for safety and shutdown management.",
     )
@@ -60,7 +60,7 @@ def generate_launch_description():
         "shutdown_hosts_config_path",
         default_value=PathJoinSubstitution(
             [
-                FindPackageShare("panther_manager"),
+                FindPackageShare("husarion_ugv_manager"),
                 "config",
                 "shutdown_hosts.yaml",
             ]
@@ -76,11 +76,11 @@ def generate_launch_description():
     )
 
     lights_manager_node = Node(
-        package="panther_manager",
+        package="husarion_ugv_manager",
         executable="lights_manager_node",
         name="lights_manager",
         parameters=[
-            PathJoinSubstitution([panther_manager_dir, "config", "lights_manager.yaml"]),
+            PathJoinSubstitution([husarion_ugv_manager_dir, "config", "lights_manager.yaml"]),
             {"bt_project_path": lights_bt_project_path},
         ],
         namespace=namespace,
@@ -88,11 +88,11 @@ def generate_launch_description():
     )
 
     safety_manager_node = Node(
-        package="panther_manager",
+        package="husarion_ugv_manager",
         executable="safety_manager_node",
         name="safety_manager",
         parameters=[
-            PathJoinSubstitution([panther_manager_dir, "config", "safety_manager.yaml"]),
+            PathJoinSubstitution([husarion_ugv_manager_dir, "config", "safety_manager.yaml"]),
             {
                 "bt_project_path": safety_bt_project_path,
                 "shutdown_hosts_path": shutdown_hosts_config_path,
diff --git a/panther_manager/package.xml b/husarion_ugv_manager/package.xml
similarity index 92%
rename from panther_manager/package.xml
rename to husarion_ugv_manager/package.xml
index 8310427d..3b5688d0 100644
--- a/panther_manager/package.xml
+++ b/husarion_ugv_manager/package.xml
@@ -1,10 +1,10 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>panther_manager</name>
+  <name>husarion_ugv_manager</name>
 
   <version>2.1.1</version>
-  <description>Set of nodes used for high level management of Husarion Panther robot</description>
+  <description>Set of nodes used for high level management of Husarion UGV</description>
   <maintainer email="support@husarion.com">Husarion</maintainer>
   <license>Apache License 2.0</license>
 
@@ -19,11 +19,11 @@
 
   <depend>behaviortree_cpp</depend>
   <depend>behaviortree_ros2</depend>
+  <depend>husarion_ugv_utils</depend>
   <depend>iputils-ping</depend>
   <depend>libboost-dev</depend>
   <depend>libssh-dev</depend>
   <depend>panther_msgs</depend>
-  <depend>panther_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>std_srvs</depend>
diff --git a/panther_manager/src/behavior_tree_manager.cpp b/husarion_ugv_manager/src/behavior_tree_manager.cpp
similarity index 94%
rename from panther_manager/src/behavior_tree_manager.cpp
rename to husarion_ugv_manager/src/behavior_tree_manager.cpp
index f39a3646..c5beb1a4 100644
--- a/panther_manager/src/behavior_tree_manager.cpp
+++ b/husarion_ugv_manager/src/behavior_tree_manager.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <panther_manager/behavior_tree_manager.hpp>
+#include <husarion_ugv_manager/behavior_tree_manager.hpp>
 
 #include <any>
 #include <map>
@@ -24,7 +24,7 @@
 #include "behaviortree_cpp/loggers/groot2_publisher.h"
 #include "rclcpp/rclcpp.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 void BehaviorTreeManager::Initialize(BT::BehaviorTreeFactory & factory)
@@ -66,4 +66,4 @@ BT::NodeConfig BehaviorTreeManager::CreateBTConfig(
   return config;
 }
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
diff --git a/panther_manager/src/lights_manager_node.cpp b/husarion_ugv_manager/src/lights_manager_node.cpp
similarity index 93%
rename from panther_manager/src/lights_manager_node.cpp
rename to husarion_ugv_manager/src/lights_manager_node.cpp
index 53ff612b..e4dbe992 100644
--- a/panther_manager/src/lights_manager_node.cpp
+++ b/husarion_ugv_manager/src/lights_manager_node.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/lights_manager_node.hpp"
+#include "husarion_ugv_manager/lights_manager_node.hpp"
 
 #include <any>
 #include <chrono>
@@ -26,12 +26,12 @@
 #include "behaviortree_ros2/ros_node_params.hpp"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_utils/moving_average.hpp"
+#include "husarion_ugv_utils/moving_average.hpp"
 
-#include <panther_manager/behavior_tree_manager.hpp>
-#include <panther_manager/behavior_tree_utils.hpp>
+#include <husarion_ugv_manager/behavior_tree_manager.hpp>
+#include <husarion_ugv_manager/behavior_tree_utils.hpp>
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 LightsManagerNode::LightsManagerNode(
@@ -45,7 +45,7 @@ LightsManagerNode::LightsManagerNode(
   const auto battery_percent_window_len =
     this->get_parameter("battery.percent.window_len").as_int();
 
-  battery_percent_ma_ = std::make_unique<panther_utils::MovingAverage<double>>(
+  battery_percent_ma_ = std::make_unique<husarion_ugv_utils::MovingAverage<double>>(
     battery_percent_window_len, 1.0);
 
   const auto initial_blackboard = CreateLightsInitialBlackboard();
@@ -81,10 +81,10 @@ void LightsManagerNode::Initialize()
 
 void LightsManagerNode::DeclareParameters()
 {
-  const auto panther_manager_pkg_path =
-    ament_index_cpp::get_package_share_directory("panther_manager");
-  const std::string default_bt_project_path = panther_manager_pkg_path +
-                                              "/behavior_trees/PantherLightsBT.btproj";
+  const auto husarion_ugv_manager_pkg_path =
+    ament_index_cpp::get_package_share_directory("husarion_ugv_manager");
+  const std::string default_bt_project_path = husarion_ugv_manager_pkg_path +
+                                              "/behavior_trees/LightsBT.btproj";
   const std::vector<std::string> default_plugin_libs = {};
 
   this->declare_parameter<std::string>("bt_project_path", default_bt_project_path);
@@ -232,4 +232,4 @@ bool LightsManagerNode::SystemReady()
   return true;
 }
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
diff --git a/panther_manager/src/lights_manager_node_main.cpp b/husarion_ugv_manager/src/lights_manager_node_main.cpp
similarity index 86%
rename from panther_manager/src/lights_manager_node_main.cpp
rename to husarion_ugv_manager/src/lights_manager_node_main.cpp
index 92a2738b..9fbbc3c8 100644
--- a/panther_manager/src/lights_manager_node_main.cpp
+++ b/husarion_ugv_manager/src/lights_manager_node_main.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/lights_manager_node.hpp"
+#include "husarion_ugv_manager/lights_manager_node.hpp"
 
 #include <iostream>
 #include <memory>
@@ -22,7 +22,8 @@ int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
 
-  auto lights_manager_node = std::make_shared<panther_manager::LightsManagerNode>("lights_manager");
+  auto lights_manager_node =
+    std::make_shared<husarion_ugv_manager::LightsManagerNode>("lights_manager");
   lights_manager_node->Initialize();
 
   try {
diff --git a/panther_manager/src/plugins/action/call_set_bool_service_node.cpp b/husarion_ugv_manager/src/plugins/action/call_set_bool_service_node.cpp
similarity index 84%
rename from panther_manager/src/plugins/action/call_set_bool_service_node.cpp
rename to husarion_ugv_manager/src/plugins/action/call_set_bool_service_node.cpp
index 561e2a5f..b51b58b8 100644
--- a/panther_manager/src/plugins/action/call_set_bool_service_node.cpp
+++ b/husarion_ugv_manager/src/plugins/action/call_set_bool_service_node.cpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/plugins/action/call_set_bool_service_node.hpp"
+#include "husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp"
 
-#include "panther_manager/behavior_tree_utils.hpp"
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 bool CallSetBoolService::setRequest(typename Request::SharedPtr & request)
@@ -42,7 +42,7 @@ BT::NodeStatus CallSetBoolService::onResponseReceived(const typename Response::S
   return BT::NodeStatus::SUCCESS;
 }
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
 #include "behaviortree_ros2/plugins.hpp"
-CreateRosNodePlugin(panther_manager::CallSetBoolService, "CallSetBoolService");
+CreateRosNodePlugin(husarion_ugv_manager::CallSetBoolService, "CallSetBoolService");
diff --git a/panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp b/husarion_ugv_manager/src/plugins/action/call_set_led_animation_service_node.cpp
similarity index 86%
rename from panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp
rename to husarion_ugv_manager/src/plugins/action/call_set_led_animation_service_node.cpp
index b0f22dd8..70c07642 100644
--- a/panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp
+++ b/husarion_ugv_manager/src/plugins/action/call_set_led_animation_service_node.cpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/plugins/action/call_set_led_animation_service_node.hpp"
+#include "husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp"
 
 #include <string>
 
 #include "behaviortree_cpp/basic_types.h"
 
-#include "panther_manager/behavior_tree_utils.hpp"
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 bool CallSetLedAnimationService::setRequest(typename Request::SharedPtr & request)
@@ -62,7 +62,7 @@ BT::NodeStatus CallSetLedAnimationService::onResponseReceived(
   return BT::NodeStatus::SUCCESS;
 }
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
 #include "behaviortree_ros2/plugins.hpp"
-CreateRosNodePlugin(panther_manager::CallSetLedAnimationService, "CallSetLedAnimationService");
+CreateRosNodePlugin(husarion_ugv_manager::CallSetLedAnimationService, "CallSetLedAnimationService");
diff --git a/panther_manager/src/plugins/action/call_trigger_service_node.cpp b/husarion_ugv_manager/src/plugins/action/call_trigger_service_node.cpp
similarity index 83%
rename from panther_manager/src/plugins/action/call_trigger_service_node.cpp
rename to husarion_ugv_manager/src/plugins/action/call_trigger_service_node.cpp
index e24b8463..73c70856 100644
--- a/panther_manager/src/plugins/action/call_trigger_service_node.cpp
+++ b/husarion_ugv_manager/src/plugins/action/call_trigger_service_node.cpp
@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/plugins/action/call_trigger_service_node.hpp"
+#include "husarion_ugv_manager/plugins/action/call_trigger_service_node.hpp"
 
 #include "behaviortree_cpp/basic_types.h"
 
-#include "panther_manager/behavior_tree_utils.hpp"
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 bool CallTriggerService::setRequest(typename Request::SharedPtr & /*request*/) { return true; }
@@ -37,7 +37,7 @@ BT::NodeStatus CallTriggerService::onResponseReceived(const typename Response::S
   return BT::NodeStatus::SUCCESS;
 }
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
 #include "behaviortree_ros2/plugins.hpp"
-CreateRosNodePlugin(panther_manager::CallTriggerService, "CallTriggerService");
+CreateRosNodePlugin(husarion_ugv_manager::CallTriggerService, "CallTriggerService");
diff --git a/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp b/husarion_ugv_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp
similarity index 71%
rename from panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp
rename to husarion_ugv_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp
index 270c67a5..a8dcfb38 100644
--- a/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp
+++ b/husarion_ugv_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp"
+#include "husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp"
 
 #include <memory>
 #include <string>
@@ -24,10 +24,10 @@
 #include "behaviortree_cpp/tree_node.h"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_manager/behavior_tree_utils.hpp"
-#include "panther_manager/plugins/shutdown_host.hpp"
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
+#include "husarion_ugv_manager/plugins/shutdown_host.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 bool ShutdownHostsFromFile::UpdateHosts(std::vector<std::shared_ptr<ShutdownHost>> & hosts)
@@ -59,13 +59,13 @@ bool ShutdownHostsFromFile::UpdateHosts(std::vector<std::shared_ptr<ShutdownHost
         continue;
       }
 
-      const auto ip = panther_utils::GetYAMLKeyValue<std::string>(host, "ip");
-      const auto username = panther_utils::GetYAMLKeyValue<std::string>(host, "username");
-      const auto port = panther_utils::GetYAMLKeyValue<unsigned>(host, "port", 22);
-      const auto command = panther_utils::GetYAMLKeyValue<std::string>(
+      const auto ip = husarion_ugv_utils::GetYAMLKeyValue<std::string>(host, "ip");
+      const auto username = husarion_ugv_utils::GetYAMLKeyValue<std::string>(host, "username");
+      const auto port = husarion_ugv_utils::GetYAMLKeyValue<unsigned>(host, "port", 22);
+      const auto command = husarion_ugv_utils::GetYAMLKeyValue<std::string>(
         host, "command", "sudo shutdown now");
-      const auto timeout = panther_utils::GetYAMLKeyValue<float>(host, "timeout", 5.0);
-      const auto ping_for_success = panther_utils::GetYAMLKeyValue<bool>(
+      const auto timeout = husarion_ugv_utils::GetYAMLKeyValue<float>(host, "timeout", 5.0);
+      const auto ping_for_success = husarion_ugv_utils::GetYAMLKeyValue<bool>(
         host, "ping_for_success", true);
       hosts.push_back(
         std::make_shared<ShutdownHost>(ip, username, port, command, timeout, ping_for_success));
@@ -77,10 +77,10 @@ bool ShutdownHostsFromFile::UpdateHosts(std::vector<std::shared_ptr<ShutdownHost
   return true;
 }
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
 #include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
-  factory.registerNodeType<panther_manager::ShutdownHostsFromFile>("ShutdownHostsFromFile");
+  factory.registerNodeType<husarion_ugv_manager::ShutdownHostsFromFile>("ShutdownHostsFromFile");
 }
diff --git a/panther_manager/src/plugins/action/shutdown_single_host_node.cpp b/husarion_ugv_manager/src/plugins/action/shutdown_single_host_node.cpp
similarity index 86%
rename from panther_manager/src/plugins/action/shutdown_single_host_node.cpp
rename to husarion_ugv_manager/src/plugins/action/shutdown_single_host_node.cpp
index 5c4f7eae..a2dbabb9 100644
--- a/panther_manager/src/plugins/action/shutdown_single_host_node.cpp
+++ b/husarion_ugv_manager/src/plugins/action/shutdown_single_host_node.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/plugins/action/shutdown_single_host_node.hpp"
+#include "husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp"
 
 #include <memory>
 #include <string>
@@ -21,10 +21,10 @@
 #include "behaviortree_cpp/exceptions.h"
 #include "behaviortree_cpp/tree_node.h"
 
-#include "panther_manager/behavior_tree_utils.hpp"
-#include "panther_manager/plugins/shutdown_host.hpp"
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
+#include "husarion_ugv_manager/plugins/shutdown_host.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 bool ShutdownSingleHost::UpdateHosts(std::vector<std::shared_ptr<ShutdownHost>> & hosts)
@@ -71,10 +71,10 @@ bool ShutdownSingleHost::UpdateHosts(std::vector<std::shared_ptr<ShutdownHost>>
   return true;
 }
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
 #include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
-  factory.registerNodeType<panther_manager::ShutdownSingleHost>("ShutdownSingleHost");
+  factory.registerNodeType<husarion_ugv_manager::ShutdownSingleHost>("ShutdownSingleHost");
 }
diff --git a/panther_manager/src/plugins/action/signal_shutdown_node.cpp b/husarion_ugv_manager/src/plugins/action/signal_shutdown_node.cpp
similarity index 83%
rename from panther_manager/src/plugins/action/signal_shutdown_node.cpp
rename to husarion_ugv_manager/src/plugins/action/signal_shutdown_node.cpp
index 8d674def..d9a5fd02 100644
--- a/panther_manager/src/plugins/action/signal_shutdown_node.cpp
+++ b/husarion_ugv_manager/src/plugins/action/signal_shutdown_node.cpp
@@ -12,14 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/plugins/action/signal_shutdown_node.hpp"
+#include "husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp"
 
 #include <string>
 #include <utility>
 
 #include "behaviortree_cpp/basic_types.h"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 BT::NodeStatus SignalShutdown::tick()
@@ -34,10 +34,10 @@ BT::NodeStatus SignalShutdown::tick()
   return BT::NodeStatus::SUCCESS;
 }
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
 #include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
-  factory.registerNodeType<panther_manager::SignalShutdown>("SignalShutdown");
+  factory.registerNodeType<husarion_ugv_manager::SignalShutdown>("SignalShutdown");
 }
diff --git a/panther_manager/src/plugins/decorator/tick_after_timeout_node.cpp b/husarion_ugv_manager/src/plugins/decorator/tick_after_timeout_node.cpp
similarity index 87%
rename from panther_manager/src/plugins/decorator/tick_after_timeout_node.cpp
rename to husarion_ugv_manager/src/plugins/decorator/tick_after_timeout_node.cpp
index 8a746ec3..e89ebb9a 100644
--- a/panther_manager/src/plugins/decorator/tick_after_timeout_node.cpp
+++ b/husarion_ugv_manager/src/plugins/decorator/tick_after_timeout_node.cpp
@@ -12,9 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/plugins/decorator/tick_after_timeout_node.hpp"
+#include "husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 TickAfterTimeout::TickAfterTimeout(const std::string & name, const BT::NodeConfig & conf)
@@ -53,10 +53,10 @@ BT::NodeStatus TickAfterTimeout::tick()
   return child_status;
 }
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
 #include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
-  factory.registerNodeType<panther_manager::TickAfterTimeout>("TickAfterTimeout");
+  factory.registerNodeType<husarion_ugv_manager::TickAfterTimeout>("TickAfterTimeout");
 }
diff --git a/panther_manager/src/safety_manager_node.cpp b/husarion_ugv_manager/src/safety_manager_node.cpp
similarity index 93%
rename from panther_manager/src/safety_manager_node.cpp
rename to husarion_ugv_manager/src/safety_manager_node.cpp
index b0500684..78283e6b 100644
--- a/panther_manager/src/safety_manager_node.cpp
+++ b/husarion_ugv_manager/src/safety_manager_node.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/safety_manager_node.hpp"
+#include "husarion_ugv_manager/safety_manager_node.hpp"
 
 #include <algorithm>
 #include <any>
@@ -28,12 +28,12 @@
 #include "behaviortree_ros2/ros_node_params.hpp"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_utils/moving_average.hpp"
+#include "husarion_ugv_utils/moving_average.hpp"
 
-#include <panther_manager/behavior_tree_manager.hpp>
-#include <panther_manager/behavior_tree_utils.hpp>
+#include <husarion_ugv_manager/behavior_tree_manager.hpp>
+#include <husarion_ugv_manager/behavior_tree_utils.hpp>
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 SafetyManagerNode::SafetyManagerNode(
@@ -48,8 +48,8 @@ SafetyManagerNode::SafetyManagerNode(
   const auto cpu_temp_window_len = this->get_parameter("cpu.temp.window_len").as_int();
 
   battery_temp_ma_ =
-    std::make_unique<panther_utils::MovingAverage<double>>(battery_temp_window_len);
-  cpu_temp_ma_ = std::make_unique<panther_utils::MovingAverage<double>>(cpu_temp_window_len);
+    std::make_unique<husarion_ugv_utils::MovingAverage<double>>(battery_temp_window_len);
+  cpu_temp_ma_ = std::make_unique<husarion_ugv_utils::MovingAverage<double>>(cpu_temp_window_len);
 
   const auto safety_initial_blackboard = CreateSafetyInitialBlackboard();
   safety_tree_manager_ = std::make_unique<BehaviorTreeManager>(
@@ -108,10 +108,10 @@ void SafetyManagerNode::Initialize()
 
 void SafetyManagerNode::DeclareParameters()
 {
-  const auto panther_manager_pkg_path =
-    ament_index_cpp::get_package_share_directory("panther_manager");
-  const std::string default_bt_project_path = panther_manager_pkg_path +
-                                              "/behavior_trees/PantherSafetyBT.btproj";
+  const auto husarion_ugv_manager_pkg_path =
+    ament_index_cpp::get_package_share_directory("husarion_ugv_manager");
+  const std::string default_bt_project_path = husarion_ugv_manager_pkg_path +
+                                              "/behavior_trees/SafetyBT.btproj";
   const std::vector<std::string> default_plugin_libs = {};
 
   this->declare_parameter<std::string>("bt_project_path", default_bt_project_path);
@@ -223,7 +223,7 @@ void SafetyManagerNode::RobotDriverStateCB(const RobotDriverStateMsg::SharedPtr
         this->get_logger(), "Creating moving average for driver '%s'", driver.name.c_str());
       const auto driver_temp_window_len = this->get_parameter("driver.temp.window_len").as_int();
       driver_temp_ma_[driver.name] =
-        std::make_unique<panther_utils::MovingAverage<double>>(driver_temp_window_len);
+        std::make_unique<husarion_ugv_utils::MovingAverage<double>>(driver_temp_window_len);
     }
 
     driver_temp_ma_[driver.name]->Roll(driver.state.temperature);
@@ -322,4 +322,4 @@ void SafetyManagerNode::ShutdownRobot(const std::string & reason)
   rclcpp::shutdown();
 }
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
diff --git a/panther_manager/src/safety_manager_node_main.cpp b/husarion_ugv_manager/src/safety_manager_node_main.cpp
similarity index 86%
rename from panther_manager/src/safety_manager_node_main.cpp
rename to husarion_ugv_manager/src/safety_manager_node_main.cpp
index 3ad34404..68486c86 100644
--- a/panther_manager/src/safety_manager_node_main.cpp
+++ b/husarion_ugv_manager/src/safety_manager_node_main.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/safety_manager_node.hpp"
+#include "husarion_ugv_manager/safety_manager_node.hpp"
 
 #include <iostream>
 #include <memory>
@@ -22,7 +22,8 @@ int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
 
-  auto safety_manager_node = std::make_shared<panther_manager::SafetyManagerNode>("safety_manager");
+  auto safety_manager_node =
+    std::make_shared<husarion_ugv_manager::SafetyManagerNode>("safety_manager");
   safety_manager_node->Initialize();
 
   try {
diff --git a/panther_manager/test/plugins/action/test_call_set_bool_service_node.cpp b/husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp
similarity index 85%
rename from panther_manager/test/plugins/action/test_call_set_bool_service_node.cpp
rename to husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp
index c9526698..e359bc59 100644
--- a/panther_manager/test/plugins/action/test_call_set_bool_service_node.cpp
+++ b/husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp
@@ -20,10 +20,10 @@
 #include "behaviortree_cpp/bt_factory.h"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_manager/plugins/action/call_set_bool_service_node.hpp"
+#include "husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp"
 #include "utils/plugin_test_utils.hpp"
 
-class TestCallSetBoolService : public panther_manager::plugin_test_utils::PluginTestUtils
+class TestCallSetBoolService : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils
 {
 public:
   void ServiceCallback(
@@ -47,7 +47,7 @@ TEST_F(TestCallSetBoolService, GoodLoadingCallSetBoolServicePlugin)
 {
   std::map<std::string, std::string> service = {{"service_name", "set_bool"}, {"data", "true"}};
 
-  RegisterNodeWithParams<panther_manager::CallSetBoolService>("CallSetBoolService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallSetBoolService>("CallSetBoolService");
 
   ASSERT_NO_THROW({ CreateTree("CallSetBoolService", service); });
 }
@@ -56,7 +56,7 @@ TEST_F(TestCallSetBoolService, WrongPluginNameLoadingCallSetBoolServicePlugin)
 {
   std::map<std::string, std::string> service = {{"service_name", "set_bool"}, {"data", "true"}};
 
-  RegisterNodeWithParams<panther_manager::CallSetBoolService>("CallSetBoolService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallSetBoolService>("CallSetBoolService");
 
   EXPECT_THROW({ CreateTree("WrongCallSetBoolService", service); }, BT::RuntimeError);
 }
@@ -65,7 +65,7 @@ TEST_F(TestCallSetBoolService, WrongCallSetBoolServiceServiceServerNotInitialize
 {
   std::map<std::string, std::string> service = {{"service_name", "set_bool"}, {"data", "true"}};
 
-  RegisterNodeWithParams<panther_manager::CallSetBoolService>("CallSetBoolService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallSetBoolService>("CallSetBoolService");
 
   CreateTree("CallSetBoolService", service);
 
@@ -86,7 +86,7 @@ TEST_F(TestCallSetBoolService, GoodSetBoolCallServiceSuccessWithTrueValue)
       ServiceCallback(request, response, true, true);
     });
 
-  RegisterNodeWithParams<panther_manager::CallSetBoolService>("CallSetBoolService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallSetBoolService>("CallSetBoolService");
 
   CreateTree("CallSetBoolService", service);
   auto & tree = GetTree();
@@ -106,7 +106,7 @@ TEST_F(TestCallSetBoolService, GoodSetBoolCallServiceSuccessWithFalseValue)
       ServiceCallback(request, response, true, false);
     });
 
-  RegisterNodeWithParams<panther_manager::CallSetBoolService>("CallSetBoolService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallSetBoolService>("CallSetBoolService");
 
   CreateTree("CallSetBoolService", service);
   auto & tree = GetTree();
@@ -126,7 +126,7 @@ TEST_F(TestCallSetBoolService, WrongSetBoolCallServiceFailure)
       ServiceCallback(request, response, false, false);
     });
 
-  RegisterNodeWithParams<panther_manager::CallSetBoolService>("CallSetBoolService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallSetBoolService>("CallSetBoolService");
 
   CreateTree("CallSetBoolService", service);
   auto & tree = GetTree();
diff --git a/panther_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp b/husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp
similarity index 86%
rename from panther_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp
rename to husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp
index d44ff4ec..1708ef7d 100644
--- a/panther_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp
+++ b/husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp
@@ -20,10 +20,11 @@
 #include "behaviortree_cpp/bt_factory.h"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_manager/plugins/action/call_set_led_animation_service_node.hpp"
+#include "husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp"
 #include "utils/plugin_test_utils.hpp"
 
-class TestCallSetLedAnimationService : public panther_manager::plugin_test_utils::PluginTestUtils
+class TestCallSetLedAnimationService
+: public husarion_ugv_manager::plugin_test_utils::PluginTestUtils
 {
 public:
   void ServiceCallback(
@@ -54,7 +55,8 @@ TEST_F(TestCallSetLedAnimationService, GoodLoadingCallSetLedAnimationServicePlug
   std::map<std::string, std::string> service = {
     {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}};
 
-  RegisterNodeWithParams<panther_manager::CallSetLedAnimationService>("CallSetLedAnimationService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallSetLedAnimationService>(
+    "CallSetLedAnimationService");
 
   ASSERT_NO_THROW({ CreateTree("CallSetLedAnimationService", service); });
 }
@@ -64,7 +66,8 @@ TEST_F(TestCallSetLedAnimationService, WrongPluginNameLoadingCallSetLedAnimation
   std::map<std::string, std::string> service = {
     {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}};
 
-  RegisterNodeWithParams<panther_manager::CallSetLedAnimationService>("CallSetLedAnimationService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallSetLedAnimationService>(
+    "CallSetLedAnimationService");
 
   EXPECT_THROW({ CreateTree("WrongCallSetLedAnimationService", service); }, BT::RuntimeError);
 }
@@ -74,7 +77,8 @@ TEST_F(TestCallSetLedAnimationService, WrongCallSetLedAnimationServiceServiceSer
   std::map<std::string, std::string> service = {
     {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}};
 
-  RegisterNodeWithParams<panther_manager::CallSetLedAnimationService>("CallSetLedAnimationService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallSetLedAnimationService>(
+    "CallSetLedAnimationService");
 
   CreateTree("CallSetLedAnimationService", service);
   auto & tree = GetTree();
@@ -88,7 +92,8 @@ TEST_F(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWith
   std::map<std::string, std::string> service = {
     {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}};
 
-  RegisterNodeWithParams<panther_manager::CallSetLedAnimationService>("CallSetLedAnimationService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallSetLedAnimationService>(
+    "CallSetLedAnimationService");
 
   CreateTree("CallSetLedAnimationService", service);
   auto & tree = GetTree();
@@ -110,7 +115,8 @@ TEST_F(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWith
   std::map<std::string, std::string> service = {
     {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "false"}};
 
-  RegisterNodeWithParams<panther_manager::CallSetLedAnimationService>("CallSetLedAnimationService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallSetLedAnimationService>(
+    "CallSetLedAnimationService");
 
   CreateTree("CallSetLedAnimationService", service);
   auto & tree = GetTree();
@@ -132,7 +138,8 @@ TEST_F(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWith
   std::map<std::string, std::string> service = {
     {"service_name", "set_led_animation"}, {"id", "5"}, {"param", ""}, {"repeating", "true"}};
 
-  RegisterNodeWithParams<panther_manager::CallSetLedAnimationService>("CallSetLedAnimationService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallSetLedAnimationService>(
+    "CallSetLedAnimationService");
 
   CreateTree("CallSetLedAnimationService", service);
   auto & tree = GetTree();
@@ -154,7 +161,8 @@ TEST_F(TestCallSetLedAnimationService, WrongSetLedAnimationCallServiceFailure)
   std::map<std::string, std::string> service = {
     {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}};
 
-  RegisterNodeWithParams<panther_manager::CallSetLedAnimationService>("CallSetLedAnimationService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallSetLedAnimationService>(
+    "CallSetLedAnimationService");
 
   CreateTree("CallSetLedAnimationService", service);
   auto & tree = GetTree();
diff --git a/panther_manager/test/plugins/action/test_call_trigger_service_node.cpp b/husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp
similarity index 84%
rename from panther_manager/test/plugins/action/test_call_trigger_service_node.cpp
rename to husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp
index e0aa3050..a130e530 100644
--- a/panther_manager/test/plugins/action/test_call_trigger_service_node.cpp
+++ b/husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp
@@ -21,10 +21,10 @@
 #include "behaviortree_cpp/bt_factory.h"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_manager/plugins/action/call_trigger_service_node.hpp"
+#include "husarion_ugv_manager/plugins/action/call_trigger_service_node.hpp"
 #include "utils/plugin_test_utils.hpp"
 
-class TestCallTriggerService : public panther_manager::plugin_test_utils::PluginTestUtils
+class TestCallTriggerService : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils
 {
 public:
   void ServiceCallback(
@@ -45,7 +45,7 @@ TEST_F(TestCallTriggerService, GoodLoadingCallTriggerServicePlugin)
 {
   std::map<std::string, std::string> service = {{"service_name", "trigger"}};
 
-  RegisterNodeWithParams<panther_manager::CallTriggerService>("CallTriggerService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallTriggerService>("CallTriggerService");
 
   ASSERT_NO_THROW({ CreateTree("CallTriggerService", service); });
 }
@@ -54,7 +54,7 @@ TEST_F(TestCallTriggerService, WrongPluginNameLoadingCallTriggerServicePlugin)
 {
   std::map<std::string, std::string> service = {{"service_name", "trigger"}};
 
-  RegisterNodeWithParams<panther_manager::CallTriggerService>("CallTriggerService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallTriggerService>("CallTriggerService");
 
   EXPECT_THROW({ CreateTree("WrongCallTriggerService", service); }, BT::RuntimeError);
 }
@@ -63,7 +63,7 @@ TEST_F(TestCallTriggerService, WrongCallTriggerServiceServiceServerNotInitialize
 {
   std::map<std::string, std::string> service = {{"service_name", "trigger"}};
 
-  RegisterNodeWithParams<panther_manager::CallTriggerService>("CallTriggerService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallTriggerService>("CallTriggerService");
 
   CreateTree("CallTriggerService", service);
   auto & tree = GetTree();
@@ -76,7 +76,7 @@ TEST_F(TestCallTriggerService, GoodTriggerCallServiceSuccess)
 {
   std::map<std::string, std::string> service = {{"service_name", "trigger"}};
 
-  RegisterNodeWithParams<panther_manager::CallTriggerService>("CallTriggerService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallTriggerService>("CallTriggerService");
 
   CreateTree("CallTriggerService", service);
   auto & tree = GetTree();
@@ -97,7 +97,7 @@ TEST_F(TestCallTriggerService, WrongTriggerCallServiceFailure)
 {
   std::map<std::string, std::string> service = {{"service_name", "trigger"}};
 
-  RegisterNodeWithParams<panther_manager::CallTriggerService>("CallTriggerService");
+  RegisterNodeWithParams<husarion_ugv_manager::CallTriggerService>("CallTriggerService");
 
   CreateTree("CallTriggerService", service);
   auto & tree = GetTree();
diff --git a/panther_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp b/husarion_ugv_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp
similarity index 77%
rename from panther_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp
rename to husarion_ugv_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp
index edece3f6..980f9e0a 100644
--- a/panther_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp
+++ b/husarion_ugv_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp
@@ -22,16 +22,16 @@
 #include "behaviortree_cpp/bt_factory.h"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp"
+#include "husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp"
 #include "utils/plugin_test_utils.hpp"
 
-typedef panther_manager::plugin_test_utils::PluginTestUtils TestShutdownHostsFromFile;
+typedef husarion_ugv_manager::plugin_test_utils::PluginTestUtils TestShutdownHostsFromFile;
 
 TEST_F(TestShutdownHostsFromFile, GoodLoadingShutdownHostsFromFilePlugin)
 {
   const std::map<std::string, std::string> service = {{"shutdown_hosts_file", "dummy_file"}};
 
-  RegisterNodeWithoutParams<panther_manager::ShutdownHostsFromFile>("ShutdownHostsFromFile");
+  RegisterNodeWithoutParams<husarion_ugv_manager::ShutdownHostsFromFile>("ShutdownHostsFromFile");
 
   ASSERT_NO_THROW({ CreateTree("ShutdownHostsFromFile", service); });
 }
@@ -40,7 +40,7 @@ TEST_F(TestShutdownHostsFromFile, WrongPluginNameLoadingShutdownHostsFromFilePlu
 {
   const std::map<std::string, std::string> service = {{"shutdown_hosts_file", "dummy_file"}};
 
-  RegisterNodeWithoutParams<panther_manager::ShutdownHostsFromFile>("ShutdownHostsFromFile");
+  RegisterNodeWithoutParams<husarion_ugv_manager::ShutdownHostsFromFile>("ShutdownHostsFromFile");
 
   EXPECT_THROW({ CreateTree("WrongShutdownHostsFromFile", service); }, BT::RuntimeError);
 }
@@ -48,10 +48,11 @@ TEST_F(TestShutdownHostsFromFile, WrongPluginNameLoadingShutdownHostsFromFilePlu
 TEST_F(TestShutdownHostsFromFile, WrongCannotFindFileShutdownHostsFromFile)
 {
   const std::string file_path =
-    testing::TempDir() + "test_panther_manager_wrong_cannot_find_file_shutdown_hosts_from_file";
+    testing::TempDir() +
+    "test_husarion_ugv_manager_wrong_cannot_find_file_shutdown_hosts_from_file";
   const std::map<std::string, std::string> service = {{"shutdown_hosts_file", file_path}};
 
-  RegisterNodeWithoutParams<panther_manager::ShutdownHostsFromFile>("ShutdownHostsFromFile");
+  RegisterNodeWithoutParams<husarion_ugv_manager::ShutdownHostsFromFile>("ShutdownHostsFromFile");
 
   CreateTree("ShutdownHostsFromFile", service);
   auto & tree = GetTree();
@@ -62,10 +63,10 @@ TEST_F(TestShutdownHostsFromFile, WrongCannotFindFileShutdownHostsFromFile)
 
 TEST_F(TestShutdownHostsFromFile, GoodShutdownHostsFromFile)
 {
-  const std::string config_file_path = testing::TempDir() +
-                                       "test_panther_manager_good_shutdown_hosts_from_file_config";
+  const std::string config_file_path =
+    testing::TempDir() + "test_husarion_ugv_manager_good_shutdown_hosts_from_file_config";
   const std::string test_file_path = testing::TempDir() +
-                                     "test_panther_manager_good_shutdown_hosts_from_file";
+                                     "test_husarion_ugv_manager_good_shutdown_hosts_from_file";
   std::filesystem::remove(test_file_path);
   std::filesystem::remove(config_file_path);
 
@@ -89,7 +90,7 @@ TEST_F(TestShutdownHostsFromFile, GoodShutdownHostsFromFile)
   config_file.close();
 
   const std::map<std::string, std::string> service = {{"shutdown_hosts_file", config_file_path}};
-  RegisterNodeWithoutParams<panther_manager::ShutdownHostsFromFile>("ShutdownHostsFromFile");
+  RegisterNodeWithoutParams<husarion_ugv_manager::ShutdownHostsFromFile>("ShutdownHostsFromFile");
 
   auto & tree = GetTree();
   CreateTree("ShutdownHostsFromFile", service);
diff --git a/panther_manager/test/plugins/action/test_shutdown_single_host_node.cpp b/husarion_ugv_manager/test/plugins/action/test_shutdown_single_host_node.cpp
similarity index 81%
rename from panther_manager/test/plugins/action/test_shutdown_single_host_node.cpp
rename to husarion_ugv_manager/test/plugins/action/test_shutdown_single_host_node.cpp
index b174b7dc..5ed922dc 100644
--- a/panther_manager/test/plugins/action/test_shutdown_single_host_node.cpp
+++ b/husarion_ugv_manager/test/plugins/action/test_shutdown_single_host_node.cpp
@@ -22,10 +22,10 @@
 #include "behaviortree_cpp/bt_factory.h"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_manager/plugins/action/shutdown_single_host_node.hpp"
+#include "husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp"
 #include "utils/plugin_test_utils.hpp"
 
-typedef panther_manager::plugin_test_utils::PluginTestUtils TestShutdownSingleHost;
+typedef husarion_ugv_manager::plugin_test_utils::PluginTestUtils TestShutdownSingleHost;
 
 TEST_F(TestShutdownSingleHost, GoodLoadingShutdownSingleHostPlugin)
 {
@@ -34,7 +34,7 @@ TEST_F(TestShutdownSingleHost, GoodLoadingShutdownSingleHostPlugin)
     {"port", "22"},     {"timeout", "5.0"},  {"username", "husarion"},
   };
 
-  RegisterNodeWithoutParams<panther_manager::ShutdownSingleHost>("ShutdownSingleHost");
+  RegisterNodeWithoutParams<husarion_ugv_manager::ShutdownSingleHost>("ShutdownSingleHost");
 
   ASSERT_NO_THROW({ CreateTree("ShutdownSingleHost", service); });
 }
@@ -46,13 +46,13 @@ TEST_F(TestShutdownSingleHost, WrongPluginNameLoadingShutdownSingleHostPlugin)
     {"port", "22"},     {"timeout", "5.0"},  {"username", "husarion"},
   };
 
-  RegisterNodeWithoutParams<panther_manager::ShutdownSingleHost>("ShutdownSingleHost");
+  RegisterNodeWithoutParams<husarion_ugv_manager::ShutdownSingleHost>("ShutdownSingleHost");
   EXPECT_THROW({ CreateTree("WrongShutdownSingleHost", service); }, BT::RuntimeError);
 }
 
 TEST_F(TestShutdownSingleHost, GoodTouchCommand)
 {
-  std::string test_file_path = testing::TempDir() + "test_panther_manager_good_touch_command";
+  std::string test_file_path = testing::TempDir() + "test_husarion_ugv_manager_good_touch_command";
   std::filesystem::remove(test_file_path);
   EXPECT_FALSE(std::filesystem::exists(test_file_path));
   std::map<std::string, std::string> service = {
@@ -63,7 +63,7 @@ TEST_F(TestShutdownSingleHost, GoodTouchCommand)
     {"timeout", "5.0"},
     {"username", "husarion"},
   };
-  RegisterNodeWithoutParams<panther_manager::ShutdownSingleHost>("ShutdownSingleHost");
+  RegisterNodeWithoutParams<husarion_ugv_manager::ShutdownSingleHost>("ShutdownSingleHost");
   CreateTree("ShutdownSingleHost", service);
   auto & tree = GetTree();
 
@@ -80,7 +80,7 @@ TEST_F(TestShutdownSingleHost, Timeout)
     {"command", "sleep 0.5"}, {"ip", "localhost"}, {"ping_for_success", "false"},
     {"port", "22"},           {"timeout", "0.1"},  {"username", "husarion"},
   };
-  RegisterNodeWithoutParams<panther_manager::ShutdownSingleHost>("ShutdownSingleHost");
+  RegisterNodeWithoutParams<husarion_ugv_manager::ShutdownSingleHost>("ShutdownSingleHost");
   CreateTree("ShutdownSingleHost", service);
   auto & tree = GetTree();
 
@@ -98,7 +98,7 @@ TEST_F(TestShutdownSingleHost, WrongUser)
     {"timeout", "0.2"},
     {"username", "wrong_user"},
   };
-  RegisterNodeWithoutParams<panther_manager::ShutdownSingleHost>("ShutdownSingleHost");
+  RegisterNodeWithoutParams<husarion_ugv_manager::ShutdownSingleHost>("ShutdownSingleHost");
 
   CreateTree("ShutdownSingleHost", service);
   auto & tree = GetTree();
diff --git a/panther_manager/test/plugins/action/test_signal_shutdown_node.cpp b/husarion_ugv_manager/test/plugins/action/test_signal_shutdown_node.cpp
similarity index 85%
rename from panther_manager/test/plugins/action/test_signal_shutdown_node.cpp
rename to husarion_ugv_manager/test/plugins/action/test_signal_shutdown_node.cpp
index 9fd109c2..3b0a383b 100644
--- a/panther_manager/test/plugins/action/test_signal_shutdown_node.cpp
+++ b/husarion_ugv_manager/test/plugins/action/test_signal_shutdown_node.cpp
@@ -21,16 +21,16 @@
 #include "behaviortree_cpp/bt_factory.h"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_manager/plugins/action/signal_shutdown_node.hpp"
+#include "husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp"
 #include "utils/plugin_test_utils.hpp"
 
-typedef panther_manager::plugin_test_utils::PluginTestUtils TestSignalShutdown;
+typedef husarion_ugv_manager::plugin_test_utils::PluginTestUtils TestSignalShutdown;
 
 TEST_F(TestSignalShutdown, GoodLoadingSignalShutdownPlugin)
 {
   std::map<std::string, std::string> service = {{"reason", "Test shutdown."}};
 
-  RegisterNodeWithoutParams<panther_manager::SignalShutdown>("SignalShutdown");
+  RegisterNodeWithoutParams<husarion_ugv_manager::SignalShutdown>("SignalShutdown");
 
   ASSERT_NO_THROW({ CreateTree("SignalShutdown", service); });
 }
@@ -46,7 +46,7 @@ TEST_F(TestSignalShutdown, GoodCheckReasonBlackboardValue)
 {
   std::map<std::string, std::string> service = {{"reason", "Test shutdown."}};
 
-  RegisterNodeWithoutParams<panther_manager::SignalShutdown>("SignalShutdown");
+  RegisterNodeWithoutParams<husarion_ugv_manager::SignalShutdown>("SignalShutdown");
 
   CreateTree("SignalShutdown", service);
   auto & tree = GetTree();
diff --git a/panther_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp b/husarion_ugv_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp
similarity index 89%
rename from panther_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp
rename to husarion_ugv_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp
index deba7db9..fb2eca46 100644
--- a/panther_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp
+++ b/husarion_ugv_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp
@@ -21,10 +21,10 @@
 #include "behaviortree_cpp/bt_factory.h"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_manager/plugins/decorator/tick_after_timeout_node.hpp"
+#include "husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp"
 #include "utils/plugin_test_utils.hpp"
 
-class TestTickAfterTimeout : public panther_manager::plugin_test_utils::PluginTestUtils
+class TestTickAfterTimeout : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils
 {
 public:
   virtual std::string BuildBehaviorTree(
@@ -51,7 +51,7 @@ std::string TestTickAfterTimeout::BuildBehaviorTree(
 
 TEST_F(TestTickAfterTimeout, GoodTickAfterTimeout)
 {
-  RegisterNodeWithoutParams<panther_manager::TickAfterTimeout>("TickAfterTimeout");
+  RegisterNodeWithoutParams<husarion_ugv_manager::TickAfterTimeout>("TickAfterTimeout");
   CreateTree("", {});
   auto & tree = GetTree();
 
diff --git a/panther_manager/test/plugins/test_shutdown_host.cpp b/husarion_ugv_manager/test/plugins/test_shutdown_host.cpp
similarity index 64%
rename from panther_manager/test/plugins/test_shutdown_host.cpp
rename to husarion_ugv_manager/test/plugins/test_shutdown_host.cpp
index 25317ee5..cd48e27d 100644
--- a/panther_manager/test/plugins/test_shutdown_host.cpp
+++ b/husarion_ugv_manager/test/plugins/test_shutdown_host.cpp
@@ -16,7 +16,7 @@
 
 #include "gtest/gtest.h"
 
-#include "panther_manager/plugins/shutdown_host.hpp"
+#include "husarion_ugv_manager/plugins/shutdown_host.hpp"
 
 class TestShutdownHost : public testing::Test
 {
@@ -26,18 +26,18 @@ class TestShutdownHost : public testing::Test
     const float timeout, const bool ping_for_success);
   bool IsAvailable();
   void Call();
-  panther_manager::ShutdownHostState GetState() const;
+  husarion_ugv_manager::ShutdownHostState GetState() const;
   std::string GetResponse() const;
 
 private:
-  std::unique_ptr<panther_manager::ShutdownHost> shutdown_host;
+  std::unique_ptr<husarion_ugv_manager::ShutdownHost> shutdown_host;
 };
 
 void TestShutdownHost::Create(
   const std::string ip, const std::string user, const int port, const std::string command,
   const float timeout, const bool ping_for_success)
 {
-  shutdown_host = std::make_unique<panther_manager::ShutdownHost>(
+  shutdown_host = std::make_unique<husarion_ugv_manager::ShutdownHost>(
     ip, user, port, command, timeout, ping_for_success);
 }
 
@@ -45,7 +45,7 @@ bool TestShutdownHost::IsAvailable() { return shutdown_host->IsAvailable(); }
 
 void TestShutdownHost::Call() { shutdown_host->Call(); }
 
-panther_manager::ShutdownHostState TestShutdownHost::GetState() const
+husarion_ugv_manager::ShutdownHostState TestShutdownHost::GetState() const
 {
   return shutdown_host->GetState();
 }
@@ -56,7 +56,7 @@ TEST_F(TestShutdownHost, GoodCheckIsAvailable)
 {
   Create("127.0.0.1", "husarion", 22, "echo HelloWorld", 0.1, true);
   EXPECT_TRUE(IsAvailable());
-  EXPECT_EQ(GetState(), panther_manager::ShutdownHostState::IDLE);
+  EXPECT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::IDLE);
 }
 
 TEST_F(TestShutdownHost, WrongCheckIsAvailable)
@@ -70,32 +70,32 @@ TEST_F(TestShutdownHost, GoodCommandExecute)
   Create("127.0.0.1", "husarion", 22, "echo HelloWorld", 0.1, false);
 
   ASSERT_TRUE(IsAvailable());
-  ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::IDLE);
+  ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::IDLE);
   Call();
-  ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::COMMAND_EXECUTED);
+  ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::COMMAND_EXECUTED);
   // Wait for response
-  while (GetState() == panther_manager::ShutdownHostState::COMMAND_EXECUTED) {
+  while (GetState() == husarion_ugv_manager::ShutdownHostState::COMMAND_EXECUTED) {
     Call();
   }
-  ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::RESPONSE_RECEIVED);
+  ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::RESPONSE_RECEIVED);
 
   const auto response = GetResponse();
   EXPECT_EQ(response, "HelloWorld\n");
 
   Call();
 
-  ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::PINGING);
+  ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::PINGING);
   Call();
-  ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::SUCCESS);
+  ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::SUCCESS);
 }
 
 TEST_F(TestShutdownHost, WrongHostPing)
 {
   Create("1.45.23.26", "husarion", 22, "echo HelloWorld", 0.1, true);
 
-  EXPECT_EQ(GetState(), panther_manager::ShutdownHostState::IDLE);
+  EXPECT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::IDLE);
   Call();
-  EXPECT_EQ(GetState(), panther_manager::ShutdownHostState::SKIPPED);
+  EXPECT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::SKIPPED);
 }
 
 TEST_F(TestShutdownHost, CheckTimeout)
@@ -103,14 +103,14 @@ TEST_F(TestShutdownHost, CheckTimeout)
   Create("127.0.0.1", "husarion", 22, "sleep 0.2", 0.1, false);
 
   ASSERT_TRUE(IsAvailable());
-  ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::IDLE);
+  ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::IDLE);
   Call();
-  ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::COMMAND_EXECUTED);
+  ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::COMMAND_EXECUTED);
   // Wait for response
-  while (GetState() == panther_manager::ShutdownHostState::COMMAND_EXECUTED) {
+  while (GetState() == husarion_ugv_manager::ShutdownHostState::COMMAND_EXECUTED) {
     Call();
   }
-  ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::FAILURE);
+  ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::FAILURE);
 }
 
 int main(int argc, char ** argv)
diff --git a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp b/husarion_ugv_manager/test/plugins/test_shutdown_hosts_node.cpp
similarity index 63%
rename from panther_manager/test/plugins/test_shutdown_hosts_node.cpp
rename to husarion_ugv_manager/test/plugins/test_shutdown_hosts_node.cpp
index 1395f439..eb797110 100644
--- a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp
+++ b/husarion_ugv_manager/test/plugins/test_shutdown_hosts_node.cpp
@@ -16,24 +16,26 @@
 
 #include "gtest/gtest.h"
 
-#include "panther_manager/plugins/shutdown_hosts_node.hpp"
+#include "husarion_ugv_manager/plugins/shutdown_hosts_node.hpp"
 
-class ShutdownHostsNodeWrapper : public panther_manager::ShutdownHosts
+class ShutdownHostsNodeWrapper : public husarion_ugv_manager::ShutdownHosts
 {
 public:
   ShutdownHostsNodeWrapper(const std::string & name, const BT::NodeConfig & conf)
-  : panther_manager::ShutdownHosts(name, conf)
+  : husarion_ugv_manager::ShutdownHosts(name, conf)
   {
   }
-  void RemoveDuplicatedHosts(std::vector<std::shared_ptr<panther_manager::ShutdownHost>> & hosts);
-  std::vector<std::shared_ptr<panther_manager::ShutdownHost>> & GetHosts();
+  void RemoveDuplicatedHosts(
+    std::vector<std::shared_ptr<husarion_ugv_manager::ShutdownHost>> & hosts);
+  std::vector<std::shared_ptr<husarion_ugv_manager::ShutdownHost>> & GetHosts();
   BT::NodeStatus onRunning();
   BT::NodeStatus onStart();
 
   virtual bool UpdateHosts(
-    std::vector<std::shared_ptr<panther_manager::ShutdownHost>> & hosts) override final;
+    std::vector<std::shared_ptr<husarion_ugv_manager::ShutdownHost>> & hosts) override final;
   void SetHostsAndSuccess(
-    std::vector<std::shared_ptr<panther_manager::ShutdownHost>> hosts, const bool returned_status);
+    std::vector<std::shared_ptr<husarion_ugv_manager::ShutdownHost>> hosts,
+    const bool returned_status);
 
   static BT::PortsList providedPorts()
   {
@@ -43,40 +45,42 @@ class ShutdownHostsNodeWrapper : public panther_manager::ShutdownHosts
   }
 
 private:
-  std::vector<std::shared_ptr<panther_manager::ShutdownHost>> hosts_to_set;
+  std::vector<std::shared_ptr<husarion_ugv_manager::ShutdownHost>> hosts_to_set;
   bool update_hosts_success_ = true;
 };
 
 void ShutdownHostsNodeWrapper::RemoveDuplicatedHosts(
-  std::vector<std::shared_ptr<panther_manager::ShutdownHost>> & hosts)
+  std::vector<std::shared_ptr<husarion_ugv_manager::ShutdownHost>> & hosts)
 {
-  panther_manager::ShutdownHosts::RemoveDuplicatedHosts(hosts);
+  husarion_ugv_manager::ShutdownHosts::RemoveDuplicatedHosts(hosts);
 }
 
-std::vector<std::shared_ptr<panther_manager::ShutdownHost>> & ShutdownHostsNodeWrapper::GetHosts()
+std::vector<std::shared_ptr<husarion_ugv_manager::ShutdownHost>> &
+ShutdownHostsNodeWrapper::GetHosts()
 {
   return hosts_;
 }
 
 BT::NodeStatus ShutdownHostsNodeWrapper::onRunning()
 {
-  return panther_manager::ShutdownHosts::onRunning();
+  return husarion_ugv_manager::ShutdownHosts::onRunning();
 }
 
 BT::NodeStatus ShutdownHostsNodeWrapper::onStart()
 {
-  return panther_manager::ShutdownHosts::onStart();
+  return husarion_ugv_manager::ShutdownHosts::onStart();
 }
 
 bool ShutdownHostsNodeWrapper::UpdateHosts(
-  std::vector<std::shared_ptr<panther_manager::ShutdownHost>> & hosts)
+  std::vector<std::shared_ptr<husarion_ugv_manager::ShutdownHost>> & hosts)
 {
   hosts = hosts_to_set;
   return update_hosts_success_;
 }
 
 void ShutdownHostsNodeWrapper::SetHostsAndSuccess(
-  std::vector<std::shared_ptr<panther_manager::ShutdownHost>> hosts, const bool returned_status)
+  std::vector<std::shared_ptr<husarion_ugv_manager::ShutdownHost>> hosts,
+  const bool returned_status)
 {
   hosts_to_set = hosts;
   update_hosts_success_ = returned_status;
@@ -86,14 +90,14 @@ class ShutdownHostsNodeTest : public testing::Test
 {
 public:
   void CreateWrapper(
-    std::vector<std::shared_ptr<panther_manager::ShutdownHost>> hosts, const bool success);
+    std::vector<std::shared_ptr<husarion_ugv_manager::ShutdownHost>> hosts, const bool success);
 
 protected:
   std::unique_ptr<ShutdownHostsNodeWrapper> wrapper;
 };
 
 void ShutdownHostsNodeTest::CreateWrapper(
-  std::vector<std::shared_ptr<panther_manager::ShutdownHost>> hosts, const bool success)
+  std::vector<std::shared_ptr<husarion_ugv_manager::ShutdownHost>> hosts, const bool success)
 {
   BT::NodeConfig conf;
   wrapper = std::make_unique<ShutdownHostsNodeWrapper>("Duplicated hosts", conf);
@@ -103,14 +107,14 @@ void ShutdownHostsNodeTest::CreateWrapper(
 TEST_F(ShutdownHostsNodeTest, GoodRemovingDuplicatedHosts)
 {
   CreateWrapper(
-    {std::make_shared<panther_manager::ShutdownHost>(
+    {std::make_shared<husarion_ugv_manager::ShutdownHost>(
        "127.0.0.1", "husarion", 22, "echo HelloWorld", 1.0, true),
-     std::make_shared<panther_manager::ShutdownHost>(
+     std::make_shared<husarion_ugv_manager::ShutdownHost>(
        "localhost", "husarion", 22, "echo HelloWorld", 1.0, true),
-     std::make_shared<panther_manager::ShutdownHost>(
+     std::make_shared<husarion_ugv_manager::ShutdownHost>(
        "localhost", "husarion", 22, "echo HelloWorld", 1.0, true)},
     true);
-  std::vector<std::shared_ptr<panther_manager::ShutdownHost>> hosts;
+  std::vector<std::shared_ptr<husarion_ugv_manager::ShutdownHost>> hosts;
   ASSERT_TRUE(wrapper->UpdateHosts(hosts));
   ASSERT_EQ(hosts.size(), 3);
   wrapper->RemoveDuplicatedHosts(hosts);
@@ -120,7 +124,7 @@ TEST_F(ShutdownHostsNodeTest, GoodRemovingDuplicatedHosts)
 TEST_F(ShutdownHostsNodeTest, FailedWhenUpdateHostReturnsFalse)
 {
   CreateWrapper(
-    {std::make_shared<panther_manager::ShutdownHost>(
+    {std::make_shared<husarion_ugv_manager::ShutdownHost>(
       "127.0.0.1", "husarion", 22, "echo HelloWorld", 1.0, true)},
     false);
 
@@ -139,7 +143,7 @@ TEST_F(ShutdownHostsNodeTest, FailedWhenHostsAreEmpty)
 TEST_F(ShutdownHostsNodeTest, CheckFailedHosts)
 {
   CreateWrapper(
-    {std::make_shared<panther_manager::ShutdownHost>(
+    {std::make_shared<husarion_ugv_manager::ShutdownHost>(
       "127.0.0.1", "husarion", 22, "echo HelloWorld", 1.0, true)},
     true);
   auto status = wrapper->onStart();
diff --git a/panther_manager/test/test_behavior_tree_manager.cpp b/husarion_ugv_manager/test/test_behavior_tree_manager.cpp
similarity index 91%
rename from panther_manager/test/test_behavior_tree_manager.cpp
rename to husarion_ugv_manager/test/test_behavior_tree_manager.cpp
index ca6925e2..80807d0e 100644
--- a/panther_manager/test/test_behavior_tree_manager.cpp
+++ b/husarion_ugv_manager/test/test_behavior_tree_manager.cpp
@@ -25,10 +25,10 @@
 #include "behaviortree_cpp/tree_node.h"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_manager/behavior_tree_manager.hpp"
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_manager/behavior_tree_manager.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
-class BehaviorTreeManagerWrapper : public panther_manager::BehaviorTreeManager
+class BehaviorTreeManagerWrapper : public husarion_ugv_manager::BehaviorTreeManager
 {
 public:
   BehaviorTreeManagerWrapper(
@@ -69,7 +69,7 @@ TEST_F(TestBehaviorTreeManager, CreateBTConfigInvalidItem)
 {
   const std::map<std::string, std::any> bb_values = {{"value", 1l}};
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::invalid_argument>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::invalid_argument>(
     [&]() { behavior_tree_manager_->CreateBTConfig(bb_values); },
     "Invalid type for blackboard entry."));
 }
@@ -109,7 +109,7 @@ TEST_F(TestBehaviorTreeManager, InitializeInvalidTreeName)
   )";
 
   ASSERT_NO_THROW(factory_.registerBehaviorTreeFromText(tree_xml));
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() { behavior_tree_manager_->Initialize(factory_); },
     "Can't find a tree with name: " + std::string(kTreeName)));
 }
diff --git a/panther_manager/test/test_behavior_tree_utils.cpp b/husarion_ugv_manager/test/test_behavior_tree_utils.cpp
similarity index 90%
rename from panther_manager/test/test_behavior_tree_utils.cpp
rename to husarion_ugv_manager/test/test_behavior_tree_utils.cpp
index 75b0cb0a..ac32b59e 100644
--- a/panther_manager/test/test_behavior_tree_utils.cpp
+++ b/husarion_ugv_manager/test/test_behavior_tree_utils.cpp
@@ -26,8 +26,8 @@
 #include "behaviortree_ros2/ros_node_params.hpp"
 #include "rclcpp/rclcpp.hpp"
 
-#include "panther_manager/behavior_tree_utils.hpp"
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
 class TestRegisterBT : public testing::Test
 {
@@ -74,7 +74,7 @@ TEST_F(TestRegisterBT, RegisterBehaviorTreeInvalidPlugin)
   BT::BehaviorTreeFactory factory;
 
   EXPECT_THROW(
-    panther_manager::behavior_tree_utils::RegisterBehaviorTree(
+    husarion_ugv_manager::behavior_tree_utils::RegisterBehaviorTree(
       factory, bt_project_path_, {"invalid_bt_node"}),
     BT::RuntimeError);
 }
@@ -83,7 +83,7 @@ TEST_F(TestRegisterBT, RegisterBehaviorTree)
 {
   BT::BehaviorTreeFactory factory;
 
-  EXPECT_NO_THROW(panther_manager::behavior_tree_utils::RegisterBehaviorTree(
+  EXPECT_NO_THROW(husarion_ugv_manager::behavior_tree_utils::RegisterBehaviorTree(
     factory, bt_project_path_, {"tick_after_timeout_bt_node", "signal_shutdown_bt_node"}));
 
   // check if nodes were registered
@@ -105,7 +105,7 @@ TEST_F(TestRegisterBT, RegisterBehaviorTreeROS)
   BT::RosNodeParams params;
   params.nh = std::make_shared<rclcpp::Node>("test_node");
 
-  EXPECT_NO_THROW(panther_manager::behavior_tree_utils::RegisterBehaviorTree(
+  EXPECT_NO_THROW(husarion_ugv_manager::behavior_tree_utils::RegisterBehaviorTree(
     factory, bt_project_path_, {}, params,
     {"call_trigger_service_bt_node", "call_set_bool_service_bt_node"}));
 
diff --git a/panther_manager/test/test_lights_behavior_tree.cpp b/husarion_ugv_manager/test/test_lights_behavior_tree.cpp
similarity index 98%
rename from panther_manager/test/test_lights_behavior_tree.cpp
rename to husarion_ugv_manager/test/test_lights_behavior_tree.cpp
index e96bce13..4a9f342d 100644
--- a/panther_manager/test/test_lights_behavior_tree.cpp
+++ b/husarion_ugv_manager/test/test_lights_behavior_tree.cpp
@@ -31,7 +31,7 @@
 #include "panther_msgs/msg/led_animation.hpp"
 #include "panther_msgs/srv/set_led_animation.hpp"
 
-#include <panther_manager/lights_manager_node.hpp>
+#include <husarion_ugv_manager/lights_manager_node.hpp>
 #include <utils/behavior_tree_test_utils.hpp>
 
 using BoolMsg = std_msgs::msg::Bool;
@@ -39,7 +39,7 @@ using BatteryStateMsg = sensor_msgs::msg::BatteryState;
 using SetLEDAnimationSrv = panther_msgs::srv::SetLEDAnimation;
 using LEDAnimationMsg = panther_msgs::msg::LEDAnimation;
 
-class LightsManagerNodeWrapper : public panther_manager::LightsManagerNode
+class LightsManagerNodeWrapper : public husarion_ugv_manager::LightsManagerNode
 {
 public:
   LightsManagerNodeWrapper(
diff --git a/panther_manager/test/test_lights_manager_node.cpp b/husarion_ugv_manager/test/test_lights_manager_node.cpp
similarity index 94%
rename from panther_manager/test/test_lights_manager_node.cpp
rename to husarion_ugv_manager/test/test_lights_manager_node.cpp
index ea60e529..cb3ef5b0 100644
--- a/panther_manager/test/test_lights_manager_node.cpp
+++ b/husarion_ugv_manager/test/test_lights_manager_node.cpp
@@ -29,13 +29,13 @@
 #include "sensor_msgs/msg/battery_state.hpp"
 #include "std_msgs/msg/bool.hpp"
 
-#include <panther_manager/lights_manager_node.hpp>
-#include "panther_utils/test/ros_test_utils.hpp"
+#include <husarion_ugv_manager/lights_manager_node.hpp>
+#include "husarion_ugv_utils/test/ros_test_utils.hpp"
 
 using BoolMsg = std_msgs::msg::Bool;
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
 
-class LightsManagerNodeWrapper : public panther_manager::LightsManagerNode
+class LightsManagerNodeWrapper : public husarion_ugv_manager::LightsManagerNode
 {
 public:
   LightsManagerNodeWrapper(
@@ -146,7 +146,7 @@ TEST_F(TestLightsManagerNode, BatteryCBBlackboardUpdate)
   battery_state.power_supply_status = expected_status;
   battery_state.power_supply_health = expected_health;
 
-  panther_utils::test_utils::PublishAndSpin(
+  husarion_ugv_utils::test_utils::PublishAndSpin(
     lights_manager_node_, "battery/battery_status", battery_state);
 
   auto blackboard = lights_manager_node_->GetLightsTreeBlackboard();
@@ -162,7 +162,7 @@ TEST_F(TestLightsManagerNode, EStopCBBlackboardUpdate)
   auto bool_msg = std_msgs::msg::Bool();
   bool_msg.data = expected_state;
 
-  panther_utils::test_utils::PublishAndSpin(
+  husarion_ugv_utils::test_utils::PublishAndSpin(
     lights_manager_node_, "hardware/e_stop", bool_msg,
     rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
 
diff --git a/panther_manager/test/test_safety_behavior_tree.cpp b/husarion_ugv_manager/test/test_safety_behavior_tree.cpp
similarity index 98%
rename from panther_manager/test/test_safety_behavior_tree.cpp
rename to husarion_ugv_manager/test/test_safety_behavior_tree.cpp
index 50a93343..6f9540bf 100644
--- a/panther_manager/test/test_safety_behavior_tree.cpp
+++ b/husarion_ugv_manager/test/test_safety_behavior_tree.cpp
@@ -33,7 +33,7 @@
 #include "panther_msgs/msg/io_state.hpp"
 #include "panther_msgs/msg/system_status.hpp"
 
-#include <panther_manager/safety_manager_node.hpp>
+#include <husarion_ugv_manager/safety_manager_node.hpp>
 #include <utils/behavior_tree_test_utils.hpp>
 
 using BoolMsg = std_msgs::msg::Bool;
@@ -43,7 +43,7 @@ using SystemStatusMsg = panther_msgs::msg::SystemStatus;
 using SetBoolSrv = std_srvs::srv::SetBool;
 using TriggerSrv = std_srvs::srv::Trigger;
 
-class SafetyManagerNodeWrapper : public panther_manager::SafetyManagerNode
+class SafetyManagerNodeWrapper : public husarion_ugv_manager::SafetyManagerNode
 {
 public:
   SafetyManagerNodeWrapper(
diff --git a/panther_manager/test/test_safety_manager_node.cpp b/husarion_ugv_manager/test/test_safety_manager_node.cpp
similarity index 94%
rename from panther_manager/test/test_safety_manager_node.cpp
rename to husarion_ugv_manager/test/test_safety_manager_node.cpp
index afd85de9..036ccdfe 100644
--- a/panther_manager/test/test_safety_manager_node.cpp
+++ b/husarion_ugv_manager/test/test_safety_manager_node.cpp
@@ -29,13 +29,13 @@
 #include "sensor_msgs/msg/battery_state.hpp"
 #include "std_msgs/msg/bool.hpp"
 
-#include <panther_manager/safety_manager_node.hpp>
-#include "panther_utils/test/ros_test_utils.hpp"
+#include <husarion_ugv_manager/safety_manager_node.hpp>
+#include "husarion_ugv_utils/test/ros_test_utils.hpp"
 
 using BoolMsg = std_msgs::msg::Bool;
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
 
-class SafetyManagerNodeWrapper : public panther_manager::SafetyManagerNode
+class SafetyManagerNodeWrapper : public husarion_ugv_manager::SafetyManagerNode
 {
 public:
   SafetyManagerNodeWrapper(
@@ -167,7 +167,7 @@ TEST_F(TestSafetyManagerNode, BatteryCBBlackboardUpdate)
   battery_state.power_supply_health = expected_health;
   battery_state.temperature = expected_temp;
 
-  panther_utils::test_utils::PublishAndSpin(
+  husarion_ugv_utils::test_utils::PublishAndSpin(
     safety_manager_node_, "battery/battery_status", battery_state);
 
   auto blackboard = safety_manager_node_->GetSafetyTreeBlackboard();
@@ -183,7 +183,7 @@ TEST_F(TestSafetyManagerNode, EStopCBBlackboardUpdate)
   auto bool_msg = std_msgs::msg::Bool();
   bool_msg.data = expected_state;
 
-  panther_utils::test_utils::PublishAndSpin(
+  husarion_ugv_utils::test_utils::PublishAndSpin(
     safety_manager_node_, "hardware/e_stop", bool_msg, transient_local_qos);
 
   auto blackboard = safety_manager_node_->GetSafetyTreeBlackboard();
@@ -200,7 +200,7 @@ TEST_F(TestSafetyManagerNode, DriverStateCBBlackboardUpdate)
   auto driver_state_msg = panther_msgs::msg::RobotDriverState();
   driver_state_msg.driver_states.push_back(driver_state);
 
-  panther_utils::test_utils::PublishAndSpin(
+  husarion_ugv_utils::test_utils::PublishAndSpin(
     safety_manager_node_, "hardware/robot_driver_state", driver_state_msg);
 
   auto blackboard = safety_manager_node_->GetSafetyTreeBlackboard();
@@ -216,7 +216,7 @@ TEST_F(TestSafetyManagerNode, IOStateCBBlackboardUpdate)
   io_state_msg.aux_power = expected_aux_state;
   io_state_msg.fan = expected_fan_state;
 
-  panther_utils::test_utils::PublishAndSpin(
+  husarion_ugv_utils::test_utils::PublishAndSpin(
     safety_manager_node_, "hardware/io_state", io_state_msg, transient_local_qos);
 
   auto blackboard = safety_manager_node_->GetSafetyTreeBlackboard();
@@ -231,7 +231,7 @@ TEST_F(TestSafetyManagerNode, SystemStatusCBBlackboardUpdate)
   auto system_status_msg = panther_msgs::msg::SystemStatus();
   system_status_msg.cpu_temp = expected_temp;
 
-  panther_utils::test_utils::PublishAndSpin(
+  husarion_ugv_utils::test_utils::PublishAndSpin(
     safety_manager_node_, "system_status", system_status_msg);
 
   auto blackboard = safety_manager_node_->GetSafetyTreeBlackboard();
diff --git a/panther_manager/test/utils/behavior_tree_test_utils.hpp b/husarion_ugv_manager/test/utils/behavior_tree_test_utils.hpp
similarity index 88%
rename from panther_manager/test/utils/behavior_tree_test_utils.hpp
rename to husarion_ugv_manager/test/utils/behavior_tree_test_utils.hpp
index 2a739811..05162ce8 100644
--- a/panther_manager/test/utils/behavior_tree_test_utils.hpp
+++ b/husarion_ugv_manager/test/utils/behavior_tree_test_utils.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_
-#define PANTHER_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_
+#ifndef HUSARION_UGV_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_
+#define HUSARION_UGV_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_
 
 #include <chrono>
 #include <functional>
@@ -57,4 +57,4 @@ bool SpinWhileRunning(
 
 }  // namespace behavior_tree::test_utils
 
-#endif  // PANTHER_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_
+#endif  // HUSARION_UGV_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_
diff --git a/panther_manager/test/utils/plugin_test_utils.hpp b/husarion_ugv_manager/test/utils/plugin_test_utils.hpp
similarity index 80%
rename from panther_manager/test/utils/plugin_test_utils.hpp
rename to husarion_ugv_manager/test/utils/plugin_test_utils.hpp
index 16a4a605..f9d4af87 100644
--- a/panther_manager/test/utils/plugin_test_utils.hpp
+++ b/husarion_ugv_manager/test/utils/plugin_test_utils.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_PLUGIN_TEST_UTILS_HPP_
-#define PANTHER_MANAGER_PLUGIN_TEST_UTILS_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGIN_TEST_UTILS_HPP_
+#define HUSARION_UGV_MANAGER_PLUGIN_TEST_UTILS_HPP_
 
 #include <functional>
 #include <limits>
@@ -32,15 +32,15 @@
 
 #include "panther_msgs/srv/set_led_animation.hpp"
 
-#include "panther_manager/plugins/action/call_set_bool_service_node.hpp"
-#include "panther_manager/plugins/action/call_set_led_animation_service_node.hpp"
-#include "panther_manager/plugins/action/call_trigger_service_node.hpp"
-#include "panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp"
-#include "panther_manager/plugins/action/shutdown_single_host_node.hpp"
-#include "panther_manager/plugins/action/signal_shutdown_node.hpp"
-#include "panther_manager/plugins/decorator/tick_after_timeout_node.hpp"
+#include "husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp"
+#include "husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp"
+#include "husarion_ugv_manager/plugins/action/call_trigger_service_node.hpp"
+#include "husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp"
+#include "husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp"
+#include "husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp"
+#include "husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp"
 
-namespace panther_manager::plugin_test_utils
+namespace husarion_ugv_manager::plugin_test_utils
 {
 
 struct BehaviorTreePluginDescription
@@ -55,7 +55,7 @@ class PluginTestUtils : public testing::Test
   PluginTestUtils()
   {
     rclcpp::init(0, nullptr);
-    bt_node_ = std::make_shared<rclcpp::Node>("test_panther_manager_node");
+    bt_node_ = std::make_shared<rclcpp::Node>("test_husarion_ugv_manager_node");
   }
 
   ~PluginTestUtils()
@@ -153,5 +153,5 @@ class PluginTestUtils : public testing::Test
       </root>
   )";
 };
-}  // namespace panther_manager::plugin_test_utils
-#endif  // PANTHER_MANAGER_PLUGIN_TEST_UTILS_HPP_
+}  // namespace husarion_ugv_manager::plugin_test_utils
+#endif  // HUSARION_UGV_MANAGER_PLUGIN_TEST_UTILS_HPP_
diff --git a/panther_utils/CHANGELOG.rst b/husarion_ugv_utils/CHANGELOG.rst
similarity index 100%
rename from panther_utils/CHANGELOG.rst
rename to husarion_ugv_utils/CHANGELOG.rst
diff --git a/panther_utils/CMakeLists.txt b/husarion_ugv_utils/CMakeLists.txt
similarity index 99%
rename from panther_utils/CMakeLists.txt
rename to husarion_ugv_utils/CMakeLists.txt
index 9fbca544..6753ec7c 100644
--- a/panther_utils/CMakeLists.txt
+++ b/husarion_ugv_utils/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10.2)
-project(panther_utils)
+project(husarion_ugv_utils)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
diff --git a/panther_utils/README.md b/husarion_ugv_utils/README.md
similarity index 52%
rename from panther_utils/README.md
rename to husarion_ugv_utils/README.md
index 78fbdf34..62ae9140 100644
--- a/panther_utils/README.md
+++ b/husarion_ugv_utils/README.md
@@ -1,3 +1,3 @@
-# panther_utils
+# husarion_ugv_utils
 
-Package containing commonly used functions, classes, and configurations for the Husarion Panther system.
+Package containing commonly used functions, classes, and configurations for the Husarion UGV system.
diff --git a/panther_utils/panther_utils/__init__.py b/husarion_ugv_utils/husarion_ugv_utils/__init__.py
similarity index 100%
rename from panther_utils/panther_utils/__init__.py
rename to husarion_ugv_utils/husarion_ugv_utils/__init__.py
diff --git a/panther_utils/panther_utils/integration_test_utils.py b/husarion_ugv_utils/husarion_ugv_utils/integration_test_utils.py
similarity index 100%
rename from panther_utils/panther_utils/integration_test_utils.py
rename to husarion_ugv_utils/husarion_ugv_utils/integration_test_utils.py
diff --git a/panther_utils/panther_utils/messages.py b/husarion_ugv_utils/husarion_ugv_utils/messages.py
similarity index 97%
rename from panther_utils/panther_utils/messages.py
rename to husarion_ugv_utils/husarion_ugv_utils/messages.py
index 33f34e22..560f1d82 100644
--- a/panther_utils/panther_utils/messages.py
+++ b/husarion_ugv_utils/husarion_ugv_utils/messages.py
@@ -63,7 +63,7 @@ def welcome_msg(
     additional_stats: Dict = {},
 ):
     """Generate a welcome message with robot information and stats."""
-    pkg_version = Command(command="ros2 pkg xml -t version panther")
+    pkg_version = Command(command="ros2 pkg xml -t version husarion_ugv")
 
     robot_model_expr = PythonExpression(
         [f"r'''{LYNX_TEXT}''' if '", robot_model, f"' == 'lynx' else r'''{PANTHER_TEXT}'''"]
diff --git a/panther_utils/include/panther_utils/common_utilities.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/common_utilities.hpp
similarity index 93%
rename from panther_utils/include/panther_utils/common_utilities.hpp
rename to husarion_ugv_utils/include/husarion_ugv_utils/common_utilities.hpp
index 237f24b5..a82b0927 100644
--- a/panther_utils/include/panther_utils/common_utilities.hpp
+++ b/husarion_ugv_utils/include/husarion_ugv_utils/common_utilities.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_UTILS_COMMON_UTILITIES_HPP_
-#define PANTHER_UTILS_COMMON_UTILITIES_HPP_
+#ifndef HUSARION_UGV_UTILS_COMMON_UTILITIES_HPP_
+#define HUSARION_UGV_UTILS_COMMON_UTILITIES_HPP_
 
 #include <cmath>
 #include <fstream>
@@ -21,7 +21,7 @@
 #include <sstream>
 #include <string>
 
-namespace panther_utils::common_utilities
+namespace husarion_ugv_utils::common_utilities
 {
 
 /**
@@ -121,6 +121,6 @@ inline bool MeetsVersionRequirement(const float version, const float required_ve
   return version >= required_version - std::numeric_limits<float>::epsilon();
 }
 
-}  // namespace panther_utils::common_utilities
+}  // namespace husarion_ugv_utils::common_utilities
 
-#endif  // PANTHER_UTILS_COMMON_UTILITIES_HPP_
+#endif  // HUSARION_UGV_UTILS_COMMON_UTILITIES_HPP_
diff --git a/panther_utils/include/panther_utils/configure_rt.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/configure_rt.hpp
similarity index 87%
rename from panther_utils/include/panther_utils/configure_rt.hpp
rename to husarion_ugv_utils/include/husarion_ugv_utils/configure_rt.hpp
index 1d6d58ae..6420b6bf 100644
--- a/panther_utils/include/panther_utils/configure_rt.hpp
+++ b/husarion_ugv_utils/include/husarion_ugv_utils/configure_rt.hpp
@@ -12,14 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_UTILS_CONFIGURE_RT_HPP_
-#define PANTHER_UTILS_CONFIGURE_RT_HPP_
+#ifndef HUSARION_UGV_UTILS_CONFIGURE_RT_HPP_
+#define HUSARION_UGV_UTILS_CONFIGURE_RT_HPP_
 
 #include <stdexcept>
 
 #include "realtime_tools/thread_priority.hpp"
 
-namespace panther_utils
+namespace husarion_ugv_utils
 {
 /**
  * @brief Configures thread that calls this function to FIFO scheduler with RT priority
@@ -44,6 +44,6 @@ inline void ConfigureRT(const unsigned priority)
   }
 }
 
-}  // namespace panther_utils
+}  // namespace husarion_ugv_utils
 
-#endif  // PANTHER_UTILS_CONFIGURE_RT_HPP_
+#endif  // HUSARION_UGV_UTILS_CONFIGURE_RT_HPP_
diff --git a/panther_utils/include/panther_utils/diagnostics.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/diagnostics.hpp
similarity index 88%
rename from panther_utils/include/panther_utils/diagnostics.hpp
rename to husarion_ugv_utils/include/husarion_ugv_utils/diagnostics.hpp
index 937a8fc2..f1278f4c 100644
--- a/panther_utils/include/panther_utils/diagnostics.hpp
+++ b/husarion_ugv_utils/include/husarion_ugv_utils/diagnostics.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_UTILS_DIAGNOSTICS_HPP_
-#define PANTHER_UTILS_DIAGNOSTICS_HPP_
+#ifndef HUSARION_UGV_UTILS_DIAGNOSTICS_HPP_
+#define HUSARION_UGV_UTILS_DIAGNOSTICS_HPP_
 
 #include <map>
 #include <string>
 
 #include "diagnostic_updater/diagnostic_status_wrapper.hpp"
 
-namespace panther_utils::diagnostics
+namespace husarion_ugv_utils::diagnostics
 {
 
 /**
@@ -49,6 +49,6 @@ void AddKeyValueIfTrue(
   }
 }
 
-}  // namespace panther_utils::diagnostics
+}  // namespace husarion_ugv_utils::diagnostics
 
-#endif  // PANTHER_UTILS_DIAGNOSTICS_HPP_
+#endif  // HUSARION_UGV_UTILS_DIAGNOSTICS_HPP_
diff --git a/panther_utils/include/panther_utils/moving_average.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
similarity index 86%
rename from panther_utils/include/panther_utils/moving_average.hpp
rename to husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
index 7575c562..0a6c7636 100644
--- a/panther_utils/include/panther_utils/moving_average.hpp
+++ b/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_UTILS_MOVING_AVERAGE_HPP_
-#define PANTHER_UTILS_MOVING_AVERAGE_HPP_
+#ifndef HUSARION_UGV_UTILS_MOVING_AVERAGE_HPP_
+#define HUSARION_UGV_UTILS_MOVING_AVERAGE_HPP_
 
 #include <deque>
 
-namespace panther_utils
+namespace husarion_ugv_utils
 {
 
 template <typename T>
@@ -61,6 +61,6 @@ class MovingAverage
   T sum_;
 };
 
-}  // namespace panther_utils
+}  // namespace husarion_ugv_utils
 
-#endif  // PANTHER_UTILS_MOVING_AVERAGE_HPP_
+#endif  // HUSARION_UGV_UTILS_MOVING_AVERAGE_HPP_
diff --git a/panther_utils/include/panther_utils/ros_utils.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/ros_utils.hpp
similarity index 94%
rename from panther_utils/include/panther_utils/ros_utils.hpp
rename to husarion_ugv_utils/include/husarion_ugv_utils/ros_utils.hpp
index d8157bc1..6d226ca7 100644
--- a/panther_utils/include/panther_utils/ros_utils.hpp
+++ b/husarion_ugv_utils/include/husarion_ugv_utils/ros_utils.hpp
@@ -12,14 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_UTILS__ROS_UTILS_HPP_
-#define PANTHER_UTILS__ROS_UTILS_HPP_
+#ifndef HUSARION_UGV_UTILS__ROS_UTILS_HPP_
+#define HUSARION_UGV_UTILS__ROS_UTILS_HPP_
 
 #include <chrono>
 
 #include "std_msgs/msg/header.hpp"
 
-namespace panther_utils::ros
+namespace husarion_ugv_utils::ros
 {
 
 /**
@@ -105,6 +105,6 @@ std::string AddNamespaceToFrameID(const std::string & frame_id, const std::strin
   return tf_prefix + frame_id;
 }
 
-}  // namespace panther_utils::ros
+}  // namespace husarion_ugv_utils::ros
 
-#endif  // PANTHER_UTILS__ROS_UTILS_HPP_
+#endif  // HUSARION_UGV_UTILS__ROS_UTILS_HPP_
diff --git a/panther_utils/include/panther_utils/test/ros_test_utils.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/test/ros_test_utils.hpp
similarity index 93%
rename from panther_utils/include/panther_utils/test/ros_test_utils.hpp
rename to husarion_ugv_utils/include/husarion_ugv_utils/test/ros_test_utils.hpp
index c6a6b728..aaef6de1 100644
--- a/panther_utils/include/panther_utils/test/ros_test_utils.hpp
+++ b/husarion_ugv_utils/include/husarion_ugv_utils/test/ros_test_utils.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_UTILS_TEST_UTILS_HPP_
-#define PANTHER_UTILS_TEST_UTILS_HPP_
+#ifndef HUSARION_UGV_UTILS_TEST_UTILS_HPP_
+#define HUSARION_UGV_UTILS_TEST_UTILS_HPP_
 
 #include <chrono>
 #include <limits>
@@ -22,7 +22,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-namespace panther_utils::test_utils
+namespace husarion_ugv_utils::test_utils
 {
 
 /**
@@ -100,6 +100,6 @@ void PublishAndSpin(
   std::this_thread::sleep_for(std::chrono::milliseconds(10));
 }
 
-}  // namespace panther_utils::test_utils
+}  // namespace husarion_ugv_utils::test_utils
 
-#endif  // PANTHER_UTILS_TEST_UTILS_HPP_
+#endif  // HUSARION_UGV_UTILS_TEST_UTILS_HPP_
diff --git a/panther_utils/include/panther_utils/test/test_utils.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/test/test_utils.hpp
similarity index 90%
rename from panther_utils/include/panther_utils/test/test_utils.hpp
rename to husarion_ugv_utils/include/husarion_ugv_utils/test/test_utils.hpp
index a71322c0..1346f5f4 100644
--- a/panther_utils/include/panther_utils/test/test_utils.hpp
+++ b/husarion_ugv_utils/include/husarion_ugv_utils/test/test_utils.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_UTILS_TEST_UTILS_HPP_
-#define PANTHER_UTILS_TEST_UTILS_HPP_
+#ifndef HUSARION_UGV_UTILS_TEST_UTILS_HPP_
+#define HUSARION_UGV_UTILS_TEST_UTILS_HPP_
 
 #include <algorithm>
 #include <cmath>
@@ -22,7 +22,7 @@
 #include <string>
 #include <vector>
 
-namespace panther_utils::test_utils
+namespace husarion_ugv_utils::test_utils
 {
 
 /**
@@ -69,6 +69,6 @@ bool IsMessageThrown(const Func & func, const std::string & error_msg)
   return false;
 }
 
-}  // namespace panther_utils::test_utils
+}  // namespace husarion_ugv_utils::test_utils
 
-#endif  // PANTHER_UTILS_TEST_UTILS_HPP_
+#endif  // HUSARION_UGV_UTILS_TEST_UTILS_HPP_
diff --git a/panther_utils/include/panther_utils/yaml_utils.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/yaml_utils.hpp
similarity index 91%
rename from panther_utils/include/panther_utils/yaml_utils.hpp
rename to husarion_ugv_utils/include/husarion_ugv_utils/yaml_utils.hpp
index e6137bda..8d32559a 100644
--- a/panther_utils/include/panther_utils/yaml_utils.hpp
+++ b/husarion_ugv_utils/include/husarion_ugv_utils/yaml_utils.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_UTILS_YAML_UTILS_HPP_
-#define PANTHER_UTILS_YAML_UTILS_HPP_
+#ifndef HUSARION_UGV_UTILS_YAML_UTILS_HPP_
+#define HUSARION_UGV_UTILS_YAML_UTILS_HPP_
 
 #include <stdexcept>
 #include <string>
 
 #include "yaml-cpp/yaml.h"
 
-namespace panther_utils
+namespace husarion_ugv_utils
 {
 
 /**
@@ -69,6 +69,6 @@ T GetYAMLKeyValue(const YAML::Node & description, const std::string & key, const
   }
 }
 
-}  // namespace panther_utils
+}  // namespace husarion_ugv_utils
 
-#endif  // PANTHER_UTILS_YAML_UTILS_HPP_
+#endif  // HUSARION_UGV_UTILS_YAML_UTILS_HPP_
diff --git a/panther_utils/package.xml b/husarion_ugv_utils/package.xml
similarity index 97%
rename from panther_utils/package.xml
rename to husarion_ugv_utils/package.xml
index aad717ec..9c5c4fd9 100644
--- a/panther_utils/package.xml
+++ b/husarion_ugv_utils/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>panther_utils</name>
+  <name>husarion_ugv_utils</name>
   <version>2.1.1</version>
   <description>Package for commonly used functions, classes, and configurations</description>
   <maintainer email="support@husarion.com">Husarion</maintainer>
diff --git a/panther_utils/test/test_common_utilities.cpp b/husarion_ugv_utils/test/test_common_utilities.cpp
similarity index 67%
rename from panther_utils/test/test_common_utilities.cpp
rename to husarion_ugv_utils/test/test_common_utilities.cpp
index 031dd043..fe374936 100644
--- a/panther_utils/test/test_common_utilities.cpp
+++ b/husarion_ugv_utils/test/test_common_utilities.cpp
@@ -20,14 +20,14 @@
 
 #include "gtest/gtest.h"
 
-#include "panther_utils/common_utilities.hpp"
+#include "husarion_ugv_utils/common_utilities.hpp"
 
 TEST(TestSetPrecision, TwoDigitPrecision)
 {
   float value = 3.14159;
   float expected_result = 3.14;
 
-  float result = panther_utils::common_utilities::SetPrecision(value, 2);
+  float result = husarion_ugv_utils::common_utilities::SetPrecision(value, 2);
 
   EXPECT_FLOAT_EQ(expected_result, result);
 }
@@ -37,7 +37,7 @@ TEST(TestSetPrecision, ZeroDigitPrecision)
   float value = 3.54159;
   float expected_result = 4.0;
 
-  float result = panther_utils::common_utilities::SetPrecision(value, 0);
+  float result = husarion_ugv_utils::common_utilities::SetPrecision(value, 0);
 
   EXPECT_FLOAT_EQ(expected_result, result);
 }
@@ -46,7 +46,7 @@ TEST(TestSetPrecision, NegativeValue)
 {
   float value = -3.14159;
   float expected_result = -3.14;
-  float result = panther_utils::common_utilities::SetPrecision(value, 2);
+  float result = husarion_ugv_utils::common_utilities::SetPrecision(value, 2);
   EXPECT_FLOAT_EQ(expected_result, result);
 }
 
@@ -54,7 +54,7 @@ TEST(TestSetPrecision, LargeValue)
 {
   float value = 123456.789;
   float expected_result = 123456.79;
-  float result = panther_utils::common_utilities::SetPrecision(value, 2);
+  float result = husarion_ugv_utils::common_utilities::SetPrecision(value, 2);
   EXPECT_FLOAT_EQ(expected_result, result);
 }
 
@@ -64,7 +64,7 @@ TEST(TestCountPercentage, ValidValues)
   int total = 100;
   float expected_result = 25.00;
 
-  float result = panther_utils::common_utilities::CountPercentage(value, total);
+  float result = husarion_ugv_utils::common_utilities::CountPercentage(value, total);
 
   EXPECT_FLOAT_EQ(expected_result, result);
 }
@@ -75,7 +75,7 @@ TEST(TestCountPercentage, ZeroTotal)
   int total = 0;
 
   EXPECT_THROW(
-    panther_utils::common_utilities::CountPercentage(value, total), std::invalid_argument);
+    husarion_ugv_utils::common_utilities::CountPercentage(value, total), std::invalid_argument);
 }
 
 TEST(TestCountPercentage, ZeroValue)
@@ -84,7 +84,7 @@ TEST(TestCountPercentage, ZeroValue)
   int total = 100;
   float expected_result = 0.00;
 
-  float result = panther_utils::common_utilities::CountPercentage(value, total);
+  float result = husarion_ugv_utils::common_utilities::CountPercentage(value, total);
 
   EXPECT_FLOAT_EQ(expected_result, result);
 }
@@ -95,7 +95,7 @@ TEST(TestPrefixMapKeys, CorrectlyPrefixesKeys)
   std::string prefix = "prefix_";
   std::map<std::string, int> expected_map = {{"prefix_key1", 1}, {"prefix_key2", 2}};
 
-  auto result_map = panther_utils::common_utilities::PrefixMapKeys(input_map, prefix);
+  auto result_map = husarion_ugv_utils::common_utilities::PrefixMapKeys(input_map, prefix);
 
   EXPECT_EQ(result_map, expected_map);
 }
@@ -106,7 +106,7 @@ TEST(TestPrefixMapKeys, HandlesEmptyPrefix)
   std::string prefix = "";
   std::map<std::string, int> expected_map = input_map;  // The maps should be identical
 
-  auto result_map = panther_utils::common_utilities::PrefixMapKeys(input_map, prefix);
+  auto result_map = husarion_ugv_utils::common_utilities::PrefixMapKeys(input_map, prefix);
 
   EXPECT_EQ(result_map, expected_map);
 }
@@ -117,7 +117,7 @@ TEST(TestPrefixMapKeys, HandlesEmptyMap)
   std::string prefix = "prefix_";
   std::map<std::string, int> expected_map;
 
-  auto result_map = panther_utils::common_utilities::PrefixMapKeys(input_map, prefix);
+  auto result_map = husarion_ugv_utils::common_utilities::PrefixMapKeys(input_map, prefix);
 
   EXPECT_EQ(result_map, expected_map);
 }
@@ -128,14 +128,14 @@ TEST(TestPrefixMapKeys, HandlesNonAlphanumericPrefix)
   std::string prefix = "prefix_@#";
   std::map<std::string, int> expected_map = {{"prefix_@#key1", 1}, {"prefix_@#key2", 2}};
 
-  auto result_map = panther_utils::common_utilities::PrefixMapKeys(input_map, prefix);
+  auto result_map = husarion_ugv_utils::common_utilities::PrefixMapKeys(input_map, prefix);
 
   EXPECT_EQ(result_map, expected_map);
 }
 
 TEST(TestOpenFile, HandleOpenFile)
 {
-  std::string path = testing::TempDir() + "test_panther_utils_open_file";
+  std::string path = testing::TempDir() + "test_husarion_ugv_utils_open_file";
 
   // Make sure that there is no random file.
   std::filesystem::remove(path);
@@ -143,26 +143,27 @@ TEST(TestOpenFile, HandleOpenFile)
   std::ofstream ofs(path);
   ofs.close();
 
-  EXPECT_NO_THROW({ panther_utils::common_utilities::OpenFile(path, std::ios_base::out); });
+  EXPECT_NO_THROW({ husarion_ugv_utils::common_utilities::OpenFile(path, std::ios_base::out); });
   std::filesystem::remove(path);
 }
 
 TEST(TestOpenFile, HandleOpenFileThrow)
 {
-  std::string path = testing::TempDir() + "test_panther_utils_open_file";
+  std::string path = testing::TempDir() + "test_husarion_ugv_utils_open_file";
 
   // Make sure that there is no random file.
   std::filesystem::remove(path);
 
   EXPECT_THROW(
-    { panther_utils::common_utilities::OpenFile(path, std::ios_base::in); }, std::runtime_error);
+    { husarion_ugv_utils::common_utilities::OpenFile(path, std::ios_base::in); },
+    std::runtime_error);
 }
 
 TEST(TestMeetsVersionRequirement, SameVersion)
 {
   float version = 1.06;
 
-  auto is_met = panther_utils::common_utilities::MeetsVersionRequirement(version, 1.06);
+  auto is_met = husarion_ugv_utils::common_utilities::MeetsVersionRequirement(version, 1.06);
 
   EXPECT_TRUE(is_met);
 }
@@ -171,7 +172,7 @@ TEST(TestMeetsVersionRequirement, LowerVersion)
 {
   float version = 1.2;
 
-  auto is_met = panther_utils::common_utilities::MeetsVersionRequirement(version, 1.06);
+  auto is_met = husarion_ugv_utils::common_utilities::MeetsVersionRequirement(version, 1.06);
 
   EXPECT_TRUE(is_met);
 }
@@ -180,7 +181,7 @@ TEST(TestMeetsVersionRequirement, HigherVersion)
 {
   float version = 1.0;
 
-  auto is_met = panther_utils::common_utilities::MeetsVersionRequirement(version, 1.06);
+  auto is_met = husarion_ugv_utils::common_utilities::MeetsVersionRequirement(version, 1.06);
 
   EXPECT_FALSE(is_met);
 }
@@ -189,7 +190,7 @@ TEST(TestMeetsVersionRequirement, NaNVersionRequired)
 {
   float version = std::numeric_limits<float>::quiet_NaN();
 
-  auto is_met = panther_utils::common_utilities::MeetsVersionRequirement(version, 1.06);
+  auto is_met = husarion_ugv_utils::common_utilities::MeetsVersionRequirement(version, 1.06);
 
   EXPECT_FALSE(is_met);
 }
diff --git a/panther_utils/test/test_diagnostics.cpp b/husarion_ugv_utils/test/test_diagnostics.cpp
similarity index 84%
rename from panther_utils/test/test_diagnostics.cpp
rename to husarion_ugv_utils/test/test_diagnostics.cpp
index c09eddc4..993275d8 100644
--- a/panther_utils/test/test_diagnostics.cpp
+++ b/husarion_ugv_utils/test/test_diagnostics.cpp
@@ -20,14 +20,14 @@
 
 #include "diagnostic_updater/diagnostic_status_wrapper.hpp"
 
-#include "panther_utils/diagnostics.hpp"
+#include "husarion_ugv_utils/diagnostics.hpp"
 
 TEST(TestAddKeyValueIfTrue, HandlesBoolean)
 {
   diagnostic_updater::DiagnosticStatusWrapper status;
   std::map<std::string, bool> kv_map = {{"key1", true}, {"key2", false}, {"key3", true}};
 
-  panther_utils::diagnostics::AddKeyValueIfTrue(status, kv_map);
+  husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(status, kv_map);
 
   ASSERT_EQ(2, status.values.size());
   EXPECT_EQ("key1", status.values.at(0).key);
@@ -40,7 +40,7 @@ TEST(TestAddKeyValueIfTrue, HandlesSmartPointer)
   std::map<std::string, std::shared_ptr<int>> kv_map = {
     {"key1", nullptr}, {"key2", nullptr}, {"key3", std::make_shared<int>(1)}};
 
-  panther_utils::diagnostics::AddKeyValueIfTrue(status, kv_map);
+  husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(status, kv_map);
 
   ASSERT_EQ(1, status.values.size());
   EXPECT_EQ("key3", status.values.at(0).key);
@@ -51,7 +51,7 @@ TEST(TestAddKeyValueIfTrue, HandlesInteger)
   diagnostic_updater::DiagnosticStatusWrapper status;
   std::map<std::string, int> kv_map = {{"key1", 0}, {"key2", 1}, {"key3", 2}};
 
-  panther_utils::diagnostics::AddKeyValueIfTrue(status, kv_map);
+  husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(status, kv_map);
 
   ASSERT_EQ(2, status.values.size());
   EXPECT_EQ("key2", status.values.at(0).key);
@@ -63,7 +63,7 @@ TEST(TestAddKeyValueIfTrue, HandlesEmptyMap)
   diagnostic_updater::DiagnosticStatusWrapper status;
   std::map<std::string, bool> kv_map;
 
-  panther_utils::diagnostics::AddKeyValueIfTrue(status, kv_map);
+  husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(status, kv_map);
 
   ASSERT_EQ(0, status.values.size());
 }
@@ -73,7 +73,7 @@ TEST(TestAddKeyValueIfTrue, HandlesPrefixAndSuffix)
   diagnostic_updater::DiagnosticStatusWrapper status;
   std::map<std::string, bool> kv_map = {{"key1", true}, {"key2", false}, {"key3", true}};
 
-  panther_utils::diagnostics::AddKeyValueIfTrue(status, kv_map, "prefix_", "_suffix");
+  husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(status, kv_map, "prefix_", "_suffix");
 
   ASSERT_EQ(2, status.values.size());
   EXPECT_EQ("prefix_key1_suffix", status.values.at(0).key);
diff --git a/panther_utils/test/test_moving_average.cpp b/husarion_ugv_utils/test/test_moving_average.cpp
similarity index 80%
rename from panther_utils/test/test_moving_average.cpp
rename to husarion_ugv_utils/test/test_moving_average.cpp
index f102a7b2..653483ce 100644
--- a/panther_utils/test/test_moving_average.cpp
+++ b/husarion_ugv_utils/test/test_moving_average.cpp
@@ -14,29 +14,29 @@
 
 #include "gtest/gtest.h"
 
-#include "panther_utils/moving_average.hpp"
+#include "husarion_ugv_utils/moving_average.hpp"
 
 TEST(TestMovingAverage, TestDefaultInitialValue)
 {
-  panther_utils::MovingAverage<double> ma(4);
+  husarion_ugv_utils::MovingAverage<double> ma(4);
   EXPECT_EQ(0.0, ma.GetAverage());
 }
 
 TEST(TestMovingAverage, TestIntialValue)
 {
-  panther_utils::MovingAverage<float> ma(4, 1.0);
+  husarion_ugv_utils::MovingAverage<float> ma(4, 1.0);
   EXPECT_EQ(1.0, ma.GetAverage());
 
-  panther_utils::MovingAverage<double> ma_1(10, 3.7);
+  husarion_ugv_utils::MovingAverage<double> ma_1(10, 3.7);
   EXPECT_EQ(3.7, ma_1.GetAverage());
 
-  panther_utils::MovingAverage<int> ma_2(3, 4);
+  husarion_ugv_utils::MovingAverage<int> ma_2(3, 4);
   EXPECT_EQ(4, ma_2.GetAverage());
 }
 
 TEST(TestMovingAverage, TestOutputValues)
 {
-  panther_utils::MovingAverage<double> ma(4);
+  husarion_ugv_utils::MovingAverage<double> ma(4);
   ma.Roll(1.0);
   ASSERT_EQ(1.0, ma.GetAverage());
 
@@ -57,7 +57,7 @@ TEST(TestMovingAverage, TestOutputValues)
 TEST(TestMovingAverage, TestHighOverload)
 {
   const std::size_t window_len = 1000;
-  panther_utils::MovingAverage<double> ma(window_len);
+  husarion_ugv_utils::MovingAverage<double> ma(window_len);
 
   double sum = 0.0;
   for (std::size_t i = 1; i <= window_len * 10; i++) {
@@ -74,7 +74,7 @@ TEST(TestMovingAverage, TestHighOverload)
 
 TEST(TestMovingAverage, TestIntFloorRound)
 {
-  panther_utils::MovingAverage<int> ma;
+  husarion_ugv_utils::MovingAverage<int> ma;
   ma.Roll(1);
   ma.Roll(2);
   EXPECT_EQ(1, ma.GetAverage());
@@ -82,7 +82,7 @@ TEST(TestMovingAverage, TestIntFloorRound)
 
 TEST(TestMovingAverage, TestReset)
 {
-  panther_utils::MovingAverage<double> ma(4);
+  husarion_ugv_utils::MovingAverage<double> ma(4);
   ma.Roll(1.0);
   ma.Roll(2.0);
   ma.Roll(1.0);
@@ -99,7 +99,7 @@ TEST(TestMovingAverage, TestReset)
 
 TEST(TestMovingAverage, TestResetToInitialValue)
 {
-  panther_utils::MovingAverage<double> ma(4, 7.0);
+  husarion_ugv_utils::MovingAverage<double> ma(4, 7.0);
   ma.Roll(1.0);
   ma.Roll(2.0);
   EXPECT_EQ(1.5, ma.GetAverage());
diff --git a/panther_utils/test/test_ros_test_utils.cpp b/husarion_ugv_utils/test/test_ros_test_utils.cpp
similarity index 84%
rename from panther_utils/test/test_ros_test_utils.cpp
rename to husarion_ugv_utils/test/test_ros_test_utils.cpp
index d47d2d13..04ea84ac 100644
--- a/panther_utils/test/test_ros_test_utils.cpp
+++ b/husarion_ugv_utils/test/test_ros_test_utils.cpp
@@ -21,7 +21,7 @@
 
 #include "std_msgs/msg/empty.hpp"
 
-#include "panther_utils/test/ros_test_utils.hpp"
+#include "husarion_ugv_utils/test/ros_test_utils.hpp"
 
 TEST(TestROSTestUtils, WaitForMessage)
 {
@@ -34,11 +34,11 @@ TEST(TestROSTestUtils, WaitForMessage)
     topic_name, 10, [&](const std_msgs::msg::Empty::SharedPtr msg) { empty_msg = msg; });
 
   EXPECT_FALSE(
-    panther_utils::test_utils::WaitForMsg(node, empty_msg, std::chrono::milliseconds(1000)));
+    husarion_ugv_utils::test_utils::WaitForMsg(node, empty_msg, std::chrono::milliseconds(1000)));
 
   pub->publish(std_msgs::msg::Empty());
   EXPECT_TRUE(
-    panther_utils::test_utils::WaitForMsg(node, empty_msg, std::chrono::milliseconds(1000)));
+    husarion_ugv_utils::test_utils::WaitForMsg(node, empty_msg, std::chrono::milliseconds(1000)));
 }
 
 TEST(TestROSTestUtils, PublishAndSpin)
@@ -53,7 +53,7 @@ TEST(TestROSTestUtils, PublishAndSpin)
 
   EXPECT_FALSE(received_msg);
 
-  panther_utils::test_utils::PublishAndSpin(node, topic_name, published_msg);
+  husarion_ugv_utils::test_utils::PublishAndSpin(node, topic_name, published_msg);
 
   EXPECT_TRUE(received_msg);
 }
diff --git a/panther_utils/test/test_ros_utils.cpp b/husarion_ugv_utils/test/test_ros_utils.cpp
similarity index 76%
rename from panther_utils/test/test_ros_utils.cpp
rename to husarion_ugv_utils/test/test_ros_utils.cpp
index 5832388a..eb3e470d 100644
--- a/panther_utils/test/test_ros_utils.cpp
+++ b/husarion_ugv_utils/test/test_ros_utils.cpp
@@ -17,7 +17,7 @@
 
 #include <std_msgs/msg/header.hpp>
 
-#include "panther_utils/ros_utils.hpp"
+#include "husarion_ugv_utils/ros_utils.hpp"
 
 using HeaderMsg = std_msgs::msg::Header;
 
@@ -32,7 +32,8 @@ TEST(TestVerifyTimestampGap, TimestampGapOk)
   header_2.stamp.nanosec = 500000000;
 
   auto max_timestamp_gap = std::chrono::seconds(2);
-  EXPECT_NO_THROW(panther_utils::ros::VerifyTimestampGap(header_1, header_2, max_timestamp_gap));
+  EXPECT_NO_THROW(
+    husarion_ugv_utils::ros::VerifyTimestampGap(header_1, header_2, max_timestamp_gap));
 }
 
 TEST(TestVerifyTimestampGap, TimestampGapExceeding)
@@ -48,7 +49,7 @@ TEST(TestVerifyTimestampGap, TimestampGapExceeding)
   auto max_timestamp_gap = std::chrono::seconds(1);
 
   EXPECT_THROW(
-    panther_utils::ros::VerifyTimestampGap(header_1, header_2, max_timestamp_gap),
+    husarion_ugv_utils::ros::VerifyTimestampGap(header_1, header_2, max_timestamp_gap),
     std::runtime_error);
 }
 
@@ -60,7 +61,7 @@ TEST(TestVerifyTimestampGap, TimestampNotSet)
   auto max_timestamp_gap = std::chrono::seconds(1);
 
   EXPECT_THROW(
-    panther_utils::ros::VerifyTimestampGap(header_1, header_2, max_timestamp_gap),
+    husarion_ugv_utils::ros::VerifyTimestampGap(header_1, header_2, max_timestamp_gap),
     std::runtime_error);
 }
 
@@ -74,7 +75,7 @@ TEST(TestMergeHeaders, SameFrameIds)
 
   HeaderMsg merged_header;
 
-  EXPECT_NO_THROW(merged_header = panther_utils::ros::MergeHeaders(header_1, header_2));
+  EXPECT_NO_THROW(merged_header = husarion_ugv_utils::ros::MergeHeaders(header_1, header_2));
   EXPECT_STREQ("frame", merged_header.frame_id.c_str());
 }
 
@@ -86,7 +87,7 @@ TEST(TestMergeHeaders, DifferentFrameIds)
   HeaderMsg header_2;
   header_2.frame_id = "frame_2";
 
-  EXPECT_THROW(panther_utils::ros::MergeHeaders(header_1, header_2), std::runtime_error);
+  EXPECT_THROW(husarion_ugv_utils::ros::MergeHeaders(header_1, header_2), std::runtime_error);
 }
 
 TEST(TestMergeHeaders, EarlierTimestampNanosec)
@@ -99,7 +100,7 @@ TEST(TestMergeHeaders, EarlierTimestampNanosec)
   header_2.stamp.sec = 10;
   header_2.stamp.nanosec = 500000000;
 
-  auto merged_header = panther_utils::ros::MergeHeaders(header_1, header_2);
+  auto merged_header = husarion_ugv_utils::ros::MergeHeaders(header_1, header_2);
 
   EXPECT_EQ(merged_header.stamp.sec, 10);
   EXPECT_EQ(merged_header.stamp.nanosec, 200000000);
@@ -115,7 +116,7 @@ TEST(TestMergeHeaders, EarlierTimestampSec)
   header_2.stamp.sec = 10;
   header_2.stamp.nanosec = 500000000;
 
-  auto merged_header = panther_utils::ros::MergeHeaders(header_1, header_2);
+  auto merged_header = husarion_ugv_utils::ros::MergeHeaders(header_1, header_2);
 
   EXPECT_EQ(merged_header.stamp.sec, 9);
   EXPECT_EQ(merged_header.stamp.nanosec, 500000000);
@@ -131,7 +132,7 @@ TEST(TestMergeHeaders, LaterTimestamp)
   header_2.stamp.sec = 9;
   header_2.stamp.nanosec = 500000000;
 
-  auto merged_header = panther_utils::ros::MergeHeaders(header_1, header_2);
+  auto merged_header = husarion_ugv_utils::ros::MergeHeaders(header_1, header_2);
 
   EXPECT_EQ(merged_header.stamp.sec, 9);
   EXPECT_EQ(merged_header.stamp.nanosec, 500000000);
@@ -142,7 +143,7 @@ TEST(TestAddNamespaceToFrameID, WithoutNamespace)
   std::string frame_id = "frame";
   std::string ns = "";
 
-  auto result = panther_utils::ros::AddNamespaceToFrameID(frame_id, ns);
+  auto result = husarion_ugv_utils::ros::AddNamespaceToFrameID(frame_id, ns);
   EXPECT_EQ(result, "frame");
 }
 
@@ -151,7 +152,7 @@ TEST(TestAddNamespaceToFrameID, WithSlashNamespace)
   std::string frame_id = "frame";
   std::string ns = "/";
 
-  auto result = panther_utils::ros::AddNamespaceToFrameID(frame_id, ns);
+  auto result = husarion_ugv_utils::ros::AddNamespaceToFrameID(frame_id, ns);
   EXPECT_EQ(result, "frame");
 }
 
@@ -160,7 +161,7 @@ TEST(TestAddNamespaceToFrameID, WithSlashedNamespace)
   std::string frame_id = "frame";
   std::string ns = "/namespace";
 
-  auto result = panther_utils::ros::AddNamespaceToFrameID(frame_id, ns);
+  auto result = husarion_ugv_utils::ros::AddNamespaceToFrameID(frame_id, ns);
   EXPECT_EQ(result, "namespace/frame");
 }
 
@@ -169,7 +170,7 @@ TEST(TestAddNamespaceToFrameID, WithNamespace)
   std::string frame_id = "frame";
   std::string ns = "namespace";
 
-  auto result = panther_utils::ros::AddNamespaceToFrameID(frame_id, ns);
+  auto result = husarion_ugv_utils::ros::AddNamespaceToFrameID(frame_id, ns);
   EXPECT_EQ(result, "namespace/frame");
 }
 
diff --git a/panther_utils/test/test_test_utils.cpp b/husarion_ugv_utils/test/test_test_utils.cpp
similarity index 66%
rename from panther_utils/test/test_test_utils.cpp
rename to husarion_ugv_utils/test/test_test_utils.cpp
index 4f319edc..fd9b0a36 100644
--- a/panther_utils/test/test_test_utils.cpp
+++ b/husarion_ugv_utils/test/test_test_utils.cpp
@@ -18,15 +18,15 @@
 
 #include "gtest/gtest.h"
 
-#include "panther_utils/test/test_utils.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
 
 template <typename T>
 void TestCheckNaNVector()
 {
   std::vector<T> vector(10, std::numeric_limits<T>::quiet_NaN());
-  EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(vector));
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector(vector));
   vector.push_back(1.0);
-  EXPECT_FALSE(panther_utils::test_utils::CheckNaNVector(vector));
+  EXPECT_FALSE(husarion_ugv_utils::test_utils::CheckNaNVector(vector));
 }
 
 TEST(TestTestUtils, CheckNanVector)
@@ -39,46 +39,46 @@ TEST(TestTestUtils, CheckNanVector)
 
 TEST(TestTestUtils, IsMessageThrownTrue)
 {
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     []() { throw std::runtime_error("Example exception"); }, "Example exception"));
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::out_of_range>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::out_of_range>(
     []() { throw std::out_of_range("Example exception"); }, "Example exception"));
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::invalid_argument>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::invalid_argument>(
     []() { throw std::invalid_argument("Example exception"); }, "Example exception"));
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     []() { throw std::runtime_error("Example exception"); }, "Example"));
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     []() { throw std::runtime_error("Example exception"); }, "exception"));
 }
 
 TEST(TestTestUtils, IsMessageThrownDifferentException)
 {
-  EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_FALSE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     []() { throw std::out_of_range("Example exception"); }, "Example exception"));
 
-  EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown<std::out_of_range>(
+  EXPECT_FALSE(husarion_ugv_utils::test_utils::IsMessageThrown<std::out_of_range>(
     []() { throw std::invalid_argument("Example exception"); }, "Example exception"));
 
-  EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown<std::invalid_argument>(
+  EXPECT_FALSE(husarion_ugv_utils::test_utils::IsMessageThrown<std::invalid_argument>(
     []() { throw std::runtime_error("Example exception"); }, "Example exception"));
 }
 
 TEST(TestTestUtils, IsMessageThrownDifferentMessage)
 {
-  EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_FALSE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     []() { throw std::runtime_error("Example exception"); }, "Different exception message"));
 
-  EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_FALSE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     []() { throw std::runtime_error("Example exception"); }, "Example exception "));
 }
 
 TEST(TestTestUtils, IsMessageThrownNoThrow)
 {
-  EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
+  EXPECT_FALSE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
     []() { return; }, "Example exception"));
 }
 
diff --git a/panther_utils/test/test_yaml_utils.cpp b/husarion_ugv_utils/test/test_yaml_utils.cpp
similarity index 53%
rename from panther_utils/test/test_yaml_utils.cpp
rename to husarion_ugv_utils/test/test_yaml_utils.cpp
index 8f853350..2f4c62fb 100644
--- a/panther_utils/test/test_yaml_utils.cpp
+++ b/husarion_ugv_utils/test/test_yaml_utils.cpp
@@ -15,14 +15,14 @@
 #include "gtest/gtest.h"
 #include "yaml-cpp/yaml.h"
 
-#include "panther_utils/test/test_utils.hpp"
-#include "panther_utils/yaml_utils.hpp"
+#include "husarion_ugv_utils/test/test_utils.hpp"
+#include "husarion_ugv_utils/yaml_utils.hpp"
 
 TEST(TestGetYAMLKeyValue, MissingKey)
 {
   YAML::Node desc;
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [desc]() { panther_utils::GetYAMLKeyValue<YAML::Node>(desc, "key_name"); },
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [desc]() { husarion_ugv_utils::GetYAMLKeyValue<YAML::Node>(desc, "key_name"); },
     "Missing 'key_name' in description"));
 }
 
@@ -34,17 +34,19 @@ TEST(TestGetYAMLKeyValue, ConversionFailure)
   desc["string_key"] = "string";
   desc["int_vector_key"] = "[1 2 3]";
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [&]() { panther_utils::GetYAMLKeyValue<int>(desc, "float_key"); }, "Failed to convert"));
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { husarion_ugv_utils::GetYAMLKeyValue<int>(desc, "float_key"); }, "Failed to convert"));
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [&]() { panther_utils::GetYAMLKeyValue<float>(desc, "string_key"); }, "Failed to convert"));
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { husarion_ugv_utils::GetYAMLKeyValue<float>(desc, "string_key"); },
+    "Failed to convert"));
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [&]() { panther_utils::GetYAMLKeyValue<int>(desc, "int_vector_key"); }, "Failed to convert"));
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { husarion_ugv_utils::GetYAMLKeyValue<int>(desc, "int_vector_key"); },
+    "Failed to convert"));
 
-  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
-    [&]() { panther_utils::GetYAMLKeyValue<std::vector<std::string>>(desc, "string_key"); },
+  EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown<std::runtime_error>(
+    [&]() { husarion_ugv_utils::GetYAMLKeyValue<std::vector<std::string>>(desc, "string_key"); },
     "Failed to convert"));
 }
 
@@ -56,13 +58,13 @@ TEST(TestGetYAMLKeyValue, GetKey)
   desc["float_key"] = 1.5;
   desc["string_key"] = "string";
 
-  const auto int_value = panther_utils::GetYAMLKeyValue<int>(desc, "int_key");
+  const auto int_value = husarion_ugv_utils::GetYAMLKeyValue<int>(desc, "int_key");
   EXPECT_EQ(2, int_value);
 
-  const auto float_value = panther_utils::GetYAMLKeyValue<float>(desc, "float_key");
+  const auto float_value = husarion_ugv_utils::GetYAMLKeyValue<float>(desc, "float_key");
   EXPECT_EQ(1.5, float_value);
 
-  const auto str_value = panther_utils::GetYAMLKeyValue<std::string>(desc, "string_key");
+  const auto str_value = husarion_ugv_utils::GetYAMLKeyValue<std::string>(desc, "string_key");
   EXPECT_EQ("string", str_value);
 }
 
@@ -72,7 +74,7 @@ TEST(TestGetYAMLKeyValue, GetVectorKey)
 
   desc["int_vector_key"] = std::vector<int>(5, 147);
 
-  const auto value_vector = panther_utils::GetYAMLKeyValue<std::vector<int>>(
+  const auto value_vector = husarion_ugv_utils::GetYAMLKeyValue<std::vector<int>>(
     desc, "int_vector_key");
   for (auto value : value_vector) {
     EXPECT_EQ(147, value);
@@ -84,7 +86,7 @@ TEST(TestGetYAMLKeyValue, GetKeyDefaultValue)
   YAML::Node desc;
   const int default_value = 54;
 
-  const auto value = panther_utils::GetYAMLKeyValue<int>(desc, "key_name", default_value);
+  const auto value = husarion_ugv_utils::GetYAMLKeyValue<int>(desc, "key_name", default_value);
   EXPECT_EQ(default_value, value);
 }
 
diff --git a/lynx_description/launch/load_urdf.launch.py b/lynx_description/launch/load_urdf.launch.py
index 6e048f9c..cc3e6dbf 100644
--- a/lynx_description/launch/load_urdf.launch.py
+++ b/lynx_description/launch/load_urdf.launch.py
@@ -72,14 +72,14 @@ def generate_launch_description():
         "controller_config_path",
         default_value=PathJoinSubstitution(
             [
-                FindPackageShare("panther_controller"),
+                FindPackageShare("husarion_ugv_controller"),
                 "config",
                 PythonExpression(["'", wheel_type, "_controller.yaml'"]),
             ]
         ),
         description=(
             "Path to controller configuration file. By default, it is located in"
-            " 'panther_controller/config/{wheel_type}_controller.yaml'. You can also specify"
+            " 'husarion_ugv_controller/config/{wheel_type}_controller.yaml'. You can also specify"
             " the path to your custom controller configuration file here. "
         ),
     )
diff --git a/lynx_description/urdf/lynx.urdf.xacro b/lynx_description/urdf/lynx.urdf.xacro
index 557b8eae..8a78c117 100644
--- a/lynx_description/urdf/lynx.urdf.xacro
+++ b/lynx_description/urdf/lynx.urdf.xacro
@@ -10,7 +10,7 @@
   <xacro:arg name="wheel_config_file"
     default="$(find lynx_description)/config/WH05.yaml" />
   <xacro:arg name="controller_config_file"
-    default="$(find panther_controller)/config/WH05_controller.yaml" />
+    default="$(find husarion_ugv_controller)/config/WH05_controller.yaml" />
   <xacro:arg name="battery_config_file"
     default="" />
   <xacro:arg name="namespace" default="" />
diff --git a/lynx_description/urdf/lynx_macro.urdf.xacro b/lynx_description/urdf/lynx_macro.urdf.xacro
index a057816d..f835d42f 100644
--- a/lynx_description/urdf/lynx_macro.urdf.xacro
+++ b/lynx_description/urdf/lynx_macro.urdf.xacro
@@ -57,12 +57,12 @@
     <ros2_control name="${ns}lynx_system" type="system">
       <hardware>
         <xacro:if value="${use_sim}">
-          <plugin>panther_gazebo/GzPantherSystem</plugin>
+          <plugin>husarion_ugv_gazebo/EStopSystem</plugin>
           <param name="e_stop_initial_state">true</param>
         </xacro:if>
 
         <xacro:unless value="$(arg use_sim)">
-          <plugin>panther_hardware_interfaces/LynxSystem</plugin>
+          <plugin>husarion_ugv_hardware_interfaces/LynxSystem</plugin>
 
           <param name="encoder_resolution">1600</param>
 
@@ -143,7 +143,7 @@
     <xacro:unless value="${use_sim}">
       <ros2_control name="${ns}imu" type="sensor">
         <hardware>
-          <plugin>panther_hardware_interfaces/PantherImuSensor</plugin>
+          <plugin>husarion_ugv_hardware_interfaces/PhidgetImuSensor</plugin>
           <param name="serial">-1</param>
           <param name="hub_port">0</param>
           <param name="data_interval_ms">8</param>
diff --git a/panther/README.md b/panther/README.md
deleted file mode 100644
index abc2f7d2..00000000
--- a/panther/README.md
+++ /dev/null
@@ -1,3 +0,0 @@
-# panther
-
-ROS 2 Metapackage composing basic functionalities of the Husarion Panther robot with VCS Tool yaml files directing to external robot dependencies.
diff --git a/panther_controller/CONFIGURATION.md b/panther_controller/CONFIGURATION.md
deleted file mode 100644
index f541b872..00000000
--- a/panther_controller/CONFIGURATION.md
+++ /dev/null
@@ -1,9 +0,0 @@
-# panther_controller
-
-## Changing Velocity Smoothing Parameters
-
-The Panther by default uses [diff_drive_controller](https://control.ros.org/master/doc/ros2_controllers/diff_drive_controller/doc/userdoc.html) from [ros2 control](https://control.ros.org/master/index.html) or [mecanum_drive_controller](https://github.com/husarion/husarion_controllers/tree/main/mecanum_drive_controller). This controller can be customized, among others: by modifying the robot's dynamic parameters (e.g. smooth the speed or limit the robot's speed and acceleration). Its parameters can be found in the [panther_controller](https://github.com/husarion/panther_ros/tree/ros2-devel/panther_controller/config). By default, these values correspond to the upper limits of the robot's velocities and accelerations.
-
-## Changing Wheel Type
-
-Changing wheel types is possible and can be done for both the real robot and the simulation. By default, three types of wheels are supported using the launch argument `wheel_type`. If you want to use custom wheels, all you need to do is point to the new wheel and controller configuration files using the `wheel_config_path` and `controller_config_path` parameters. These files should be based on the default files, i.e. [WH01_controller.yaml](./config/WH01_controller.yaml) and [WH01.yaml](../panther_description/config/WH01.yaml).
diff --git a/panther_description/launch/load_urdf.launch.py b/panther_description/launch/load_urdf.launch.py
index 8a0f8ee6..1297f7c1 100644
--- a/panther_description/launch/load_urdf.launch.py
+++ b/panther_description/launch/load_urdf.launch.py
@@ -71,14 +71,14 @@ def generate_launch_description():
         "controller_config_path",
         default_value=PathJoinSubstitution(
             [
-                FindPackageShare("panther_controller"),
+                FindPackageShare("husarion_ugv_controller"),
                 "config",
                 PythonExpression(["'", wheel_type, "_controller.yaml'"]),
             ]
         ),
         description=(
             "Path to controller configuration file. By default, it is located in"
-            " 'panther_controller/config/{wheel_type}_controller.yaml'. You can also specify"
+            " 'husarion_ugv_controller/config/{wheel_type}_controller.yaml'. You can also specify"
             " the path to your custom controller configuration file here. "
         ),
     )
diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro
index 29e3558f..2d520a1c 100644
--- a/panther_description/urdf/gazebo.urdf.xacro
+++ b/panther_description/urdf/gazebo.urdf.xacro
@@ -109,7 +109,7 @@
   <!-- Lights -->
   <xacro:macro name="light" params="reference_frame name topic namespace:=''">
     <gazebo>
-      <plugin filename="LEDStrip" name="panther_gazebo::LEDStrip">
+      <plugin filename="LEDStrip" name="husarion_ugv_gazebo::LEDStrip">
         <light_name>${name}</light_name>
         <topic>${topic}</topic>
         <namespace>${namespace}</namespace>
diff --git a/panther_description/urdf/panther.urdf.xacro b/panther_description/urdf/panther.urdf.xacro
index 1c167186..a60b84e9 100644
--- a/panther_description/urdf/panther.urdf.xacro
+++ b/panther_description/urdf/panther.urdf.xacro
@@ -9,7 +9,7 @@
   <xacro:arg name="wheel_config_file"
     default="$(find panther_description)/config/WH01.yaml" />
   <xacro:arg name="controller_config_file"
-    default="$(find panther_controller)/config/WH01_controller.yaml" />
+    default="$(find husarion_ugv_controller)/config/WH01_controller.yaml" />
   <xacro:arg name="battery_config_file"
     default="" />
   <xacro:arg name="namespace" default="" />
diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro
index bef943de..99c7ca43 100644
--- a/panther_description/urdf/panther_macro.urdf.xacro
+++ b/panther_description/urdf/panther_macro.urdf.xacro
@@ -56,12 +56,12 @@
     <ros2_control name="${ns}panther_system" type="system">
       <hardware>
         <xacro:if value="${use_sim}">
-          <plugin>panther_gazebo/GzPantherSystem</plugin>
+          <plugin>husarion_ugv_gazebo/EStopSystem</plugin>
           <param name="e_stop_initial_state">true</param>
         </xacro:if>
 
         <xacro:unless value="${use_sim}">
-          <plugin>panther_hardware_interfaces/PantherSystem</plugin>
+          <plugin>husarion_ugv_hardware_interfaces/PantherSystem</plugin>
 
           <param name="encoder_resolution">1600</param>
 
@@ -143,7 +143,7 @@
     <xacro:unless value="${use_sim}">
       <ros2_control name="${ns}imu" type="sensor">
         <hardware>
-          <plugin>panther_hardware_interfaces/PantherImuSensor</plugin>
+          <plugin>husarion_ugv_hardware_interfaces/PhidgetImuSensor</plugin>
           <param name="serial">-1</param>
           <param name="hub_port">0</param>
           <param name="data_interval_ms">8</param>
diff --git a/panther_hardware_interfaces/panther_hardware_interfaces.xml b/panther_hardware_interfaces/panther_hardware_interfaces.xml
deleted file mode 100644
index dbb3512e..00000000
--- a/panther_hardware_interfaces/panther_hardware_interfaces.xml
+++ /dev/null
@@ -1,25 +0,0 @@
-<library path="panther_hardware_interfaces">
-  <class name="panther_hardware_interfaces/PantherSystem"
-    type="panther_hardware_interfaces::PantherSystem"
-    base_class_type="hardware_interface::SystemInterface">
-    <description>
-      Hardware system controller for Panther's Roboteq motor controller
-    </description>
-  </class>
-
-  <class name="panther_hardware_interfaces/LynxSystem"
-    type="panther_hardware_interfaces::LynxSystem"
-    base_class_type="hardware_interface::SystemInterface">
-    <description>
-      Hardware system controller for Panther's Roboteq motor controller
-    </description>
-  </class>
-
-  <class name="panther_hardware_interfaces/PantherImuSensor"
-    type="panther_hardware_interfaces::PantherImuSensor"
-    base_class_type="hardware_interface::SensorInterface">
-    <description>
-      Hardware IMU sensor for Panther's Phidget Spatial Inertial Measurement Unit
-    </description>
-  </class>
-</library>
diff --git a/panther_hardware_interfaces/test/utils/panther_system_test_utils.hpp b/panther_hardware_interfaces/test/utils/panther_system_test_utils.hpp
deleted file mode 100644
index 0e07cc37..00000000
--- a/panther_hardware_interfaces/test/utils/panther_system_test_utils.hpp
+++ /dev/null
@@ -1,164 +0,0 @@
-// Copyright 2023 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_HARDWARE_INTERFACES_TEST_UTILS_PANTHER_SYSTEM_TEST_UTILS_HPP_
-#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_PANTHER_SYSTEM_TEST_UTILS_HPP_
-
-#include <cmath>
-#include <cstdint>
-#include <map>
-#include <string>
-
-#include <gtest/gtest.h>
-
-#include <hardware_interface/resource_manager.hpp>
-#include <rclcpp/rclcpp.hpp>
-
-#include "roboteqs_mock.hpp"
-#include "test_constants.hpp"
-
-namespace panther_hardware_interfaces_test
-{
-
-/**
- * @brief Utility class for testing Panther System
- */
-class PantherSystemTestUtils
-{
-public:
-  PantherSystemTestUtils()
-  {
-    default_panther_system_urdf_ = BuildUrdf(kDefaultParamMap, kDefaultJoints);
-  }
-
-  /**
-   * @brief Starts Roboteq Mock, initializes rclcpp and creates resource manager
-   * @param urdf urdf used to create resource manager
-   */
-  void Start(const std::string & urdf)
-  {
-    roboteqs_mock_ = std::make_unique<RoboteqsMock>();
-    roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50));
-    rclcpp::init(0, nullptr);
-
-    rm_ = std::make_shared<hardware_interface::ResourceManager>(urdf);
-  }
-
-  /**
-   * @brief Shuts down rclcpp, stops Roboteq mock and destroys resource manager
-   */
-  void Stop()
-  {
-    rclcpp::shutdown();
-    roboteqs_mock_->Stop();
-    roboteqs_mock_.reset();
-    rm_.reset();
-  }
-
-  /**
-   * @brief Creates and returns URDF as a string
-   * @param param_map map with hardware parameters
-   * @param joints vector of joint names
-   */
-  std::string BuildUrdf(
-    const std::map<std::string, std::string> & param_map, const std::vector<std::string> & joints)
-  {
-    std::stringstream urdf;
-
-    urdf << kUrdfHeader << "<hardware>" << std::endl << kPluginName;
-
-    for (auto const & [key, val] : param_map) {
-      urdf << "<param name=\"" << key << "\">" << val << "</param>" << std::endl;
-    }
-
-    urdf << "</hardware>" << std::endl;
-
-    for (auto const & joint : joints) {
-      urdf << "<joint name=\"" << joint << "\">" << std::endl
-           << kJointInterfaces << "</joint>" << std::endl;
-    }
-
-    urdf << kUrdfFooter;
-
-    return urdf.str();
-  }
-
-  void ConfigurePantherSystem()
-  {
-    SetState(
-      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
-      hardware_interface::lifecycle_state_names::INACTIVE);
-  }
-
-  void UnconfigurePantherSystem()
-  {
-    SetState(
-      lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
-      hardware_interface::lifecycle_state_names::UNCONFIGURED);
-  }
-
-  void ActivatePantherSystem()
-  {
-    SetState(
-      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
-      hardware_interface::lifecycle_state_names::ACTIVE);
-  }
-
-  void DeactivatePantherSystem()
-  {
-    SetState(
-      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
-      hardware_interface::lifecycle_state_names::INACTIVE);
-  }
-
-  void ShutdownPantherSystem()
-  {
-    SetState(
-      lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
-      hardware_interface::lifecycle_state_names::FINALIZED);
-  }
-
-  void ConfigureActivatePantherSystem()
-  {
-    ConfigurePantherSystem();
-    ActivatePantherSystem();
-  }
-
-  std::shared_ptr<hardware_interface::ResourceManager> GetResourceManager() { return rm_; }
-  std::shared_ptr<RoboteqsMock> GetRoboteqsMock() { return roboteqs_mock_; }
-
-  std::string GetDefaultPantherSystemUrdf() const { return default_panther_system_urdf_; }
-
-private:
-  /**
-   * @brief Changes current state of the resource manager to the one set in parameters. It is
-   * recommended to use wrapper functions
-   * @param state_id
-   * @param state_name
-   */
-  void SetState(const std::uint8_t state_id, const std::string & state_name)
-  {
-    rclcpp_lifecycle::State state(state_id, state_name);
-    rm_->set_component_state(kPantherSystemName, state);
-  }
-
-  std::shared_ptr<RoboteqsMock> roboteqs_mock_;
-  std::shared_ptr<hardware_interface::ResourceManager> rm_;
-
-  std::string default_panther_system_urdf_;
-};
-
-}  // namespace panther_hardware_interfaces_test
-
-#endif  // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_PANTHER_SYSTEM_TEST_UTILS_HPP_
diff --git a/panther_lights/plugins.xml b/panther_lights/plugins.xml
deleted file mode 100644
index 11f0ed75..00000000
--- a/panther_lights/plugins.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-<library path="panther_animation_plugins">
-  <class type="panther_lights::ImageAnimation" base_class_type="panther_lights::Animation">
-    <description>Animation processed from an image</description>
-  </class>
-  <class type="panther_lights::ChargingAnimation" base_class_type="panther_lights::Animation">
-    <description>Charging animation representing percentage of battery</description>
-  </class>
-</library>

From caaa60ebb692b6302d16acbeefd65e68ace97b12 Mon Sep 17 00:00:00 2001
From: Dawid Kmak <73443304+KmakD@users.noreply.github.com>
Date: Mon, 14 Oct 2024 09:21:46 +0200
Subject: [PATCH 075/100] Update gpio controller (#425)

---
 .../CODE_STRUCTURE.md                         |  41 +++--
 .../robot_system/gpio/gpio_controller.hpp     | 142 ++----------------
 .../src/robot_system/gpio/gpio_controller.cpp | 122 ++-------------
 .../src/robot_system/ugv_system.cpp           |   2 +-
 .../gpio/test_gpio_controller.cpp             |   4 +-
 5 files changed, 55 insertions(+), 256 deletions(-)

diff --git a/husarion_ugv_hardware_interfaces/CODE_STRUCTURE.md b/husarion_ugv_hardware_interfaces/CODE_STRUCTURE.md
index 0b6a7b59..a2e2b9a1 100644
--- a/husarion_ugv_hardware_interfaces/CODE_STRUCTURE.md
+++ b/husarion_ugv_hardware_interfaces/CODE_STRUCTURE.md
@@ -2,20 +2,36 @@
 
 A brief introduction to the code structure of the Husarion UGV system.
 
-## RoboteqDriver
+## DriverInterface
 
-Low-level CANopen driver implementing LoopDriver from [Lely](https://opensource.lely.com/canopen/).
+Interface to manage robot driver.
+Implementations:
+
+* `RoboteqDriver`: Low-level CANopen driver implementing LoopDriver from [Lely](https://opensource.lely.com/canopen/).
 It takes care of translating CANopen indexes into meaningful data.
 Provided methods can be used for sending commands and reading all the useful parameters from the Roboteq drivers (they abstract low level SDO and PDO communication).
-Timestamp of all received PDO data is also saved, which can be later used for detecting timeout errors.
+Timestamp of all received PDO data is also saved, which can be later used for detecting timeout errors. Also, manages `MotorDrivers`.
+
+## MotorDriverInterface
+
+Abstract interface for managing each motor connected to the driver.
+Implementations:
+
+* `RoboteqMotorDriver`: Responsible for reading state and sending command velocities with usage of `RoboteqDriver` interfaces.
+
+## CANopenManager
 
-## CANopenController
+Takes care of CANopen communication - creates and initializes master controller. For handling CANopen communication separate thread is created with configurable RT priority.
 
-Takes care of CANopen communication - creates and initializes master controller and two Roboteq drivers (front and rear). For handling CANopen communication separate thread is created with configurable RT priority (additionally two threads for each driver are also created).
+## RobotDriver
 
-## MotorsController
+Interface to control robot drivers.
+Implementations:
 
-This class abstracts the usage of two Roboteq controllers. It uses canopen_controller for communication with Roboteq controllers, implements the activation procedure for controllers (resets script and sends initial 0 command), and provides methods to get data feedback and send commands. Data is converted between raw Roboteq formats and SI units using `roboteq_data_converters`.
+* `RoboteqRobotDriver`: This class abstracts the usage of Roboteq controllers. It uses canopen_controller for communication with Roboteq controllers, implements the activation procedure for controllers (resets script and sends initial 0 command), and provides methods to get data feedback and send commands. Data is converted between raw Roboteq formats and SI units using `roboteq_data_converters`.
+It has two concrete implementations:
+  * `LynxRobotDriver`: Contains one Roboteq controller.
+  * `PantherRobotDriver`: Contains two Roboteq controllers.
 
 ## RoboteqDataConverters
 
@@ -27,14 +43,14 @@ Data feedback converters also store data (it is passed using Set methods, and la
 
 * `MotorState` - converts position, velocity and torque feedback
 * `FaultFlag`, `ScriptFlag`, `RuntimeError` - converts flag error data into messages
-* `DriverState` - temperature, voltage, and current
+* `RoboteqDriverState` - temperature, voltage, and current
 
-Feedback converters are combined in the `RoboteqData` class to provide the full state of one controller. It consists of
+Feedback converters are combined in the `DriverData` class to provide the full state of one controller. It consists of
 
 * 2 `MotorState` (left and right)
 * `FaultFlag`, `ScriptFlag`
 * 2 `RuntimeError` (for left and right motors)
-* `DriverState`
+* `RoboteqDriverState`
 
 ## RoboteqErrorFilter
 
@@ -51,9 +67,8 @@ It comprises a wrapper implementation for the GPIOD library, enabling real-time
 The GPIOController provides wrappers for the GPIO driver, handling reading and writing pins of the RPi GPIO. It includes the following utilities:
 
 * `GPIOControllerInterface`: Interface for all wrappers that handle GPIO control tasks.
-* `GPIOControllerPTH12X`: Class with specific logic for the Panther robot with version 1.20 and above.
-* `GPIOControllerPTH10X`: Class with specific logic for the Panther robot with version below 1.20.
-* `Watchdog`: Entity responsible for spinning the software Watchdog. It periodically sets the high and low states of specific GPIO Watchdog pin. Used only with `GPIOControllerPTH12X`.
+* `GPIOController`: Class with specific logic for Lynx and Panther robot.
+* `Watchdog`: Entity responsible for spinning the software Watchdog. It periodically sets the high and low states of specific GPIO Watchdog pin. Used only with `GPIOController`.
 
 ## EStop
 
diff --git a/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp
index f563d250..d147de83 100644
--- a/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp
+++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp
@@ -130,7 +130,7 @@ class GPIOControllerInterface
    * };
    *
    * MyClass my_obj;
-   * GPIOControllerPTH12X gpio_controller;
+   * GPIOController gpio_controller;
    * gpio_controller.RegisterGPIOEventCallback(
    *     std::bind(&MyClass::HandleGPIOEvent, &my_obj, std::placeholders::_1));
    * @endcode
@@ -145,16 +145,16 @@ class GPIOControllerInterface
   std::shared_ptr<GPIODriverInterface> gpio_driver_;
 };
 
-class GPIOControllerPTH12X : public GPIOControllerInterface
+class GPIOController : public GPIOControllerInterface
 {
 public:
   /**
-   * @brief Constructor for GPIOControllerPTH12X class.
+   * @brief Constructor for GPIOController class.
    *
    * @param gpio_driver Pointer to the GPIODriver object.
    * @throw `std::runtime_error` When the GPIO driver is not initialized.
    */
-  GPIOControllerPTH12X(std::shared_ptr<GPIODriverInterface> gpio_driver);
+  GPIOController(std::shared_ptr<GPIODriverInterface> gpio_driver);
 
   /**
    * @brief Initializes the GPIODriver, Watchdog, and powers on the motors.
@@ -275,145 +275,21 @@ class GPIOControllerPTH12X : public GPIOControllerInterface
   volatile std::atomic_bool should_abort_e_stop_reset_ = false;
 };
 
-class GPIOControllerPTH10X : public GPIOControllerInterface
-{
-public:
-  /**
-   * @brief Constructor for GPIOControllerPTH10X class.
-   *
-   * @param gpio_driver Pointer to the GPIODriver object.
-   * @throw `std::runtime_error` When the GPIO driver is not initialized.
-   */
-  GPIOControllerPTH10X(std::shared_ptr<GPIODriverInterface> gpio_driver);
-
-  /**
-   * @brief Initializes the GPIODriver and powers on the motors.
-   */
-  void Start() override;
-
-  /**
-   * @brief Placeholder method indicating lack of hardware E-stop support for the robot in this
-   * version.
-   *
-   * @return Always returns true.
-   */
-  void EStopTrigger() override;
-
-  /**
-   * @brief Checks if the motors are powered up (when STAGE2_INPUT is active/main switch is set to
-   * STAGE2 position) without controlling any GPIO.
-   *
-   * @exception std::runtime_error when the motors are not powered up.
-   * @return Always returns true when the motors are powered up.
-   */
-  void EStopReset() override;
-
-  /**
-   * @brief Controls the motor power by enabling or disabling them based on the 'enable'
-   * parameter.
-   *
-   * This method checks if the motors are powered up by verifying the 'STAGE2_INPUT' pin.
-   *
-   * @param enable Set to 'true' to enable the motors, 'false' to disable.
-   * @exception std::runtime_error When attempting to enable the motors without the 'STAGE2_INPUT'
-   * pin active.
-   * @return 'true' if the motor control pin value is successfully set, 'false' otherwise.
-   */
-  bool MotorPowerEnable(const bool enable) override;
-
-  /**
-   * @brief Placeholder method indicating lack of support for controlling the fan in this robot
-   * version.
-   *
-   * @param enable Ignored parameter in this version.
-   * @exception std::runtime_error Always throws a runtime error due to lack of support for fan
-   * control.
-   */
-  bool FanEnable(const bool /* enable */) override;
-
-  /**
-   * @brief Placeholder method indicating lack of support for controlling AUX in this robot
-   * version.
-   *
-   * @param enable Ignored parameter in this version.
-   * @exception std::runtime_error Always throws a runtime error due to lack of support for AUX
-   * power control.
-   */
-  bool AUXPowerEnable(const bool /* enable */) override;
-
-  /**
-   * @brief Placeholder method indicating lack of support for controlling Digital Power in this
-   * robot version.
-   *
-   * @param enable Ignored parameter in this version.
-   * @exception std::runtime_error Always throws a runtime error due to lack of support for
-   * digital power control.
-   */
-  bool DigitalPowerEnable(const bool /* enable */) override;
-
-  /**
-   * @brief Placeholder method indicating lack of support for enabling external charger in this
-   * robot version.
-   *
-   * @param enable Ignored parameter in this version.
-   * @exception std::runtime_error Always throws a runtime error due to lack of support for
-   * charging process control.
-   */
-  bool ChargerEnable(const bool /* enable */) override;
-
-  /**
-   * @brief Enables or disables the LED control based on the 'enable' parameter.
-   *
-   * @param enable Set to 'true' to enable the LED control, 'false' to disable.
-   * @return 'true' if the LED control pin value is successfully set, 'false' otherwise.
-   */
-  bool LEDControlEnable(const bool enable) override;
-
-  /**
-   * @brief Returns imitation of the IO states of the control interface. In this version of the
-   * robot, there is a lack of support for controlling these IOs.
-   *
-   * @return An unordered map containing the GPIOPin as the key and its active state as the value.
-   */
-  std::unordered_map<GPIOPin, bool> QueryControlInterfaceIOStates() const override;
-
-  /**
-   * @brief Returns the GPIO pin configuration information for the PTH10X.
-   */
-  static const std::vector<GPIOInfo> & GetGPIOConfigInfoStorage();
-
-private:
-  /**
-   * @brief Vector containing GPIO pin configuration information such as pin direction, value,
-   * etc.
-   */
-  static const std::vector<GPIOInfo> gpio_config_info_storage_;
-};
-
 class GPIOControllerFactory
 {
 public:
   /**
-   * @brief Creates a GPIO controller based on the robot version.
+   * @brief Creates a GPIO controller.
    *
-   * @param robot_version The robot version to create the GPIO controller for.
    * @return A unique pointer to the created GPIO controller.
    */
-  static std::unique_ptr<GPIOControllerInterface> CreateGPIOController(const float robot_version)
+  static std::unique_ptr<GPIOControllerInterface> CreateGPIOController()
   {
     std::unique_ptr<GPIOControllerInterface> gpio_controller;
+    auto config_info_storage = GPIOController::GetGPIOConfigInfoStorage();
+    auto gpio_driver = std::make_shared<GPIODriver>(config_info_storage);
 
-    if (husarion_ugv_utils::common_utilities::MeetsVersionRequirement(robot_version, 1.2)) {
-      auto config_info_storage = GPIOControllerPTH12X::GetGPIOConfigInfoStorage();
-      auto gpio_driver = std::make_shared<GPIODriver>(config_info_storage);
-
-      gpio_controller = std::make_unique<GPIOControllerPTH12X>(gpio_driver);
-    } else {
-      auto config_info_storage = GPIOControllerPTH10X::GetGPIOConfigInfoStorage();
-      auto gpio_driver = std::make_shared<GPIODriver>(config_info_storage);
-
-      gpio_controller = std::make_unique<GPIOControllerPTH10X>(gpio_driver);
-    }
+    gpio_controller = std::make_unique<GPIOController>(gpio_driver);
 
     return gpio_controller;
   };
diff --git a/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp
index d2847ab7..f511a9af 100644
--- a/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp
@@ -102,7 +102,7 @@ bool GPIOControllerInterface::IsPinAvailable(const GPIOPin pin) const
   return gpio_driver_->IsPinAvailable(pin);
 }
 
-GPIOControllerPTH12X::GPIOControllerPTH12X(std::shared_ptr<GPIODriverInterface> gpio_driver)
+GPIOController::GPIOController(std::shared_ptr<GPIODriverInterface> gpio_driver)
 {
   gpio_driver_ = gpio_driver;
 
@@ -111,7 +111,7 @@ GPIOControllerPTH12X::GPIOControllerPTH12X(std::shared_ptr<GPIODriverInterface>
   }
 }
 
-void GPIOControllerPTH12X::Start()
+void GPIOController::Start()
 {
   gpio_driver_->GPIOMonitorEnable(true, 60);
 
@@ -121,14 +121,14 @@ void GPIOControllerPTH12X::Start()
   watchdog_ = std::make_unique<Watchdog>(gpio_driver_);
 }
 
-void GPIOControllerPTH12X::EStopTrigger()
+void GPIOController::EStopTrigger()
 {
   if (!watchdog_->TurnOff()) {
     throw std::runtime_error("Can't stop watchdog thread");
   }
 }
 
-void GPIOControllerPTH12X::EStopReset()
+void GPIOController::EStopReset()
 {
   const auto e_stop_pin = GPIOPin::E_STOP_RESET;
   bool e_stop_state = !gpio_driver_->IsPinActive(e_stop_pin);
@@ -162,37 +162,37 @@ void GPIOControllerPTH12X::EStopReset()
   }
 }
 
-bool GPIOControllerPTH12X::MotorPowerEnable(const bool enable)
+bool GPIOController::MotorPowerEnable(const bool enable)
 {
   return gpio_driver_->SetPinValue(GPIOPin::DRIVER_EN, enable);
 };
 
-bool GPIOControllerPTH12X::AUXPowerEnable(const bool enable)
+bool GPIOController::AUXPowerEnable(const bool enable)
 {
   return gpio_driver_->SetPinValue(GPIOPin::AUX_PW_EN, enable);
 };
 
-bool GPIOControllerPTH12X::FanEnable(const bool enable)
+bool GPIOController::FanEnable(const bool enable)
 {
   return gpio_driver_->SetPinValue(GPIOPin::FAN_SW, enable);
 };
 
-bool GPIOControllerPTH12X::DigitalPowerEnable(const bool enable)
+bool GPIOController::DigitalPowerEnable(const bool enable)
 {
   return gpio_driver_->SetPinValue(GPIOPin::VDIG_OFF, !enable);
 };
 
-bool GPIOControllerPTH12X::ChargerEnable(const bool enable)
+bool GPIOController::ChargerEnable(const bool enable)
 {
   return gpio_driver_->SetPinValue(GPIOPin::CHRG_DISABLE, !enable);
 }
 
-bool GPIOControllerPTH12X::LEDControlEnable(const bool enable)
+bool GPIOController::LEDControlEnable(const bool enable)
 {
   return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable);
 }
 
-std::unordered_map<GPIOPin, bool> GPIOControllerPTH12X::QueryControlInterfaceIOStates() const
+std::unordered_map<GPIOPin, bool> GPIOController::QueryControlInterfaceIOStates() const
 {
   std::unordered_map<GPIOPin, bool> io_state;
 
@@ -209,7 +209,7 @@ std::unordered_map<GPIOPin, bool> GPIOControllerPTH12X::QueryControlInterfaceIOS
   return io_state;
 }
 
-const std::vector<GPIOInfo> GPIOControllerPTH12X::gpio_config_info_storage_ = {
+const std::vector<GPIOInfo> GPIOController::gpio_config_info_storage_ = {
   GPIOInfo{GPIOPin::WATCHDOG, gpiod::line::direction::OUTPUT},
   GPIOInfo{GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT},
   GPIOInfo{GPIOPin::CHRG_DISABLE, gpiod::line::direction::OUTPUT},
@@ -227,19 +227,19 @@ const std::vector<GPIOInfo> GPIOControllerPTH12X::gpio_config_info_storage_ = {
   GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true},
 };
 
-const std::vector<GPIOInfo> & GPIOControllerPTH12X::GetGPIOConfigInfoStorage()
+const std::vector<GPIOInfo> & GPIOController::GetGPIOConfigInfoStorage()
 {
   return gpio_config_info_storage_;
 }
 
-void GPIOControllerPTH12X::InterruptEStopReset()
+void GPIOController::InterruptEStopReset()
 {
   std::lock_guard<std::mutex> lck(e_stop_cv_mtx_);
   should_abort_e_stop_reset_ = true;
   e_stop_cv_.notify_one();
 }
 
-bool GPIOControllerPTH12X::WaitFor(std::chrono::milliseconds timeout)
+bool GPIOController::WaitFor(std::chrono::milliseconds timeout)
 {
   std::unique_lock<std::mutex> lck(e_stop_cv_mtx_);
 
@@ -250,96 +250,4 @@ bool GPIOControllerPTH12X::WaitFor(std::chrono::milliseconds timeout)
   return !interrupted;
 }
 
-GPIOControllerPTH10X::GPIOControllerPTH10X(std::shared_ptr<GPIODriverInterface> gpio_driver)
-{
-  gpio_driver_ = gpio_driver;
-
-  if (!gpio_driver_) {
-    throw std::runtime_error("GPIO driver is not initialized.");
-  }
-}
-
-void GPIOControllerPTH10X::Start()
-{
-  gpio_driver_->GPIOMonitorEnable(true, 60);
-
-  gpio_driver_->SetPinValue(GPIOPin::MOTOR_ON, true);
-}
-
-void GPIOControllerPTH10X::EStopTrigger()
-{
-  throw std::runtime_error(
-    "This robot version does not support this functionality. Trying to set safety stop using CAN "
-    "command.");
-}
-
-void GPIOControllerPTH10X::EStopReset()
-{
-  if (!gpio_driver_->IsPinActive(GPIOPin::STAGE2_INPUT)) {
-    throw std::runtime_error(
-      "Motors are not powered up. Please verify if the main switch is in the 'STAGE2' position.");
-  }
-}
-
-bool GPIOControllerPTH10X::MotorPowerEnable(const bool enable)
-{
-  if (enable && !gpio_driver_->IsPinActive(GPIOPin::STAGE2_INPUT)) {
-    throw std::runtime_error(
-      "Motors are not powered up. Please verify if the main switch is in the 'STAGE2' position.");
-  }
-
-  return gpio_driver_->SetPinValue(GPIOPin::MOTOR_ON, enable);
-}
-
-bool GPIOControllerPTH10X::AUXPowerEnable(const bool /* enable */)
-{
-  throw std::runtime_error("This robot version does not support this functionality.");
-};
-
-bool GPIOControllerPTH10X::FanEnable(const bool /* enable */)
-{
-  throw std::runtime_error("This robot version does not support this functionality.");
-}
-
-bool GPIOControllerPTH10X::DigitalPowerEnable(const bool /* enable */)
-{
-  throw std::runtime_error("This robot version does not support this functionality.");
-};
-
-bool GPIOControllerPTH10X::ChargerEnable(const bool /* enable */)
-{
-  throw std::runtime_error("This robot version does not support this functionality.");
-}
-
-bool GPIOControllerPTH10X::LEDControlEnable(const bool enable)
-{
-  return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable);
-}
-
-std::unordered_map<GPIOPin, bool> GPIOControllerPTH10X::QueryControlInterfaceIOStates() const
-{
-  std::unordered_map<GPIOPin, bool> io_state;
-
-  io_state.emplace(GPIOPin::AUX_PW_EN, true);
-  io_state.emplace(GPIOPin::CHRG_SENSE, false);
-  io_state.emplace(GPIOPin::CHRG_DISABLE, false);
-  io_state.emplace(GPIOPin::VDIG_OFF, false);
-  io_state.emplace(GPIOPin::FAN_SW, false);
-  io_state.emplace(GPIOPin::SHDN_INIT, false);
-  io_state.emplace(GPIOPin::MOTOR_ON, gpio_driver_->IsPinActive(GPIOPin::MOTOR_ON));
-
-  return io_state;
-}
-
-const std::vector<GPIOInfo> GPIOControllerPTH10X::gpio_config_info_storage_ = {
-  GPIOInfo{GPIOPin::STAGE2_INPUT, gpiod::line::direction::INPUT},
-  GPIOInfo{GPIOPin::MOTOR_ON, gpiod::line::direction::OUTPUT},
-  GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true},
-};
-
-const std::vector<GPIOInfo> & GPIOControllerPTH10X::GetGPIOConfigInfoStorage()
-{
-  return gpio_config_info_storage_;
-}
-
 }  // namespace husarion_ugv_hardware_interfaces
diff --git a/husarion_ugv_hardware_interfaces/src/robot_system/ugv_system.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/ugv_system.cpp
index 8f24a571..1915cf6e 100644
--- a/husarion_ugv_hardware_interfaces/src/robot_system/ugv_system.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/ugv_system.cpp
@@ -421,7 +421,7 @@ void UGVSystem::ReadDriverStatesUpdateFrequency()
 
 void UGVSystem::ConfigureGPIOController()
 {
-  gpio_controller_ = GPIOControllerFactory::CreateGPIOController(1.2);
+  gpio_controller_ = GPIOControllerFactory::CreateGPIOController();
   gpio_controller_->Start();
 
   RCLCPP_INFO(logger_, "Successfully configured GPIO controller.");
diff --git a/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp b/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp
index 3ee2069d..8be90f2c 100644
--- a/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp
+++ b/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp
@@ -42,11 +42,11 @@ class MockGPIODriver : public husarion_ugv_hardware_interfaces::GPIODriverInterf
   MOCK_METHOD(bool, SetPinValue, (const GPIOPin pin, const bool value), (override));
 };
 
-class GPIOControllerWrapper : public husarion_ugv_hardware_interfaces::GPIOControllerPTH12X
+class GPIOControllerWrapper : public husarion_ugv_hardware_interfaces::GPIOController
 {
 public:
   GPIOControllerWrapper(std::shared_ptr<MockGPIODriver> gpio_driver)
-  : husarion_ugv_hardware_interfaces::GPIOControllerPTH12X(gpio_driver)
+  : husarion_ugv_hardware_interfaces::GPIOController(gpio_driver)
   {
   }
 

From 380f181cd16c546dc7539563819a5b46b1655cca Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Thu, 31 Oct 2024 11:38:59 +0100
Subject: [PATCH 076/100] Add RViz launch

---
 README.md                                     |   1 +
 .../launch/simulation.launch.py               |  13 +
 lynx_description/urdf/gazebo.urdf.xacro       |   7 +-
 panther_description/launch/rviz.launch.py     |  91 ++++++
 panther_description/package.xml               |   2 +
 panther_description/rviz/husarion_ugv.rviz    | 268 ++++++++++++++++++
 panther_description/rviz/panther.rviz         | 211 --------------
 panther_description/urdf/gazebo.urdf.xacro    |   7 +-
 8 files changed, 385 insertions(+), 215 deletions(-)
 create mode 100644 panther_description/launch/rviz.launch.py
 create mode 100644 panther_description/rviz/husarion_ugv.rviz
 delete mode 100644 panther_description/rviz/panther.rviz

diff --git a/README.md b/README.md
index 77b854e9..1b7dee1e 100644
--- a/README.md
+++ b/README.md
@@ -108,6 +108,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | ✅   | ✅   | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`SafetyBT.btproj`](./husarion_ugv_manager/behavior_trees/SafetyBT.btproj)                                                                                                                    |
 | ✅   | ✅   | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts.yaml`](./husarion_ugv_manager/config/shutdown_hosts.yaml)                                                                                                                                                                |
 | ✅   | ✅   | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                    |
+| ❌   | ✅   | `use_rviz`                   | Run RViz simultaneously. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                                   |
 | ✅   | ✅   | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                              |
 | ✅   | ✅   | `user_led_animations_path`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                    |
 | ✅   | ✅   | `wheel_config_path`          | Path to wheel configuration file. <br/> ***string:*** [`{wheel_type}.yaml`](./panther_description/config)                                                                                                                                                                                                          |
diff --git a/husarion_ugv_gazebo/launch/simulation.launch.py b/husarion_ugv_gazebo/launch/simulation.launch.py
index cf2e78de..cf34f986 100644
--- a/husarion_ugv_gazebo/launch/simulation.launch.py
+++ b/husarion_ugv_gazebo/launch/simulation.launch.py
@@ -59,6 +59,18 @@ def generate_launch_description():
         launch_arguments={"gz_gui": namespaced_gz_gui, "gz_log_level": "1"}.items(),
     )
 
+    rviz_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("panther_description"),
+                    "launch",
+                    "rviz.launch.py",
+                ]
+            )
+        ),
+    )
+
     simulate_robots = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -77,6 +89,7 @@ def generate_launch_description():
         # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo)
         SetUseSimTime(True),
         gz_sim,
+        rviz_launch,
         simulate_robots,
     ]
 
diff --git a/lynx_description/urdf/gazebo.urdf.xacro b/lynx_description/urdf/gazebo.urdf.xacro
index 6eea0941..ce1a25c8 100644
--- a/lynx_description/urdf/gazebo.urdf.xacro
+++ b/lynx_description/urdf/gazebo.urdf.xacro
@@ -42,12 +42,15 @@
         <parameters>${config_file}</parameters>
         <ros>
           <namespace>${namespace}</namespace>
+          <remapping>drive_controller/cmd_vel_unstamped:=cmd_vel</remapping>
+          <remapping>drive_controller/odom:=odometry/wheels</remapping>
+          <remapping>drive_controller/transition_event:=drive_controller/_transition_event</remapping>
           <remapping>gz_ros2_control/e_stop:=hardware/e_stop</remapping>
           <remapping>gz_ros2_control/e_stop_reset:=hardware/e_stop_reset</remapping>
           <remapping>gz_ros2_control/e_stop_trigger:=hardware/e_stop_trigger</remapping>
           <remapping>imu_broadcaster/imu:=imu/data</remapping>
-          <remapping>drive_controller/cmd_vel_unstamped:=cmd_vel</remapping>
-          <remapping>drive_controller/odom:=odometry/wheels</remapping>
+          <remapping>imu_broadcaster/transition_event:=imu_broadcaster/_transition_event</remapping>
+          <remapping>joint_state_broadcaster/transition_event:=joint_state_broadcaster/_transition_event</remapping>
         </ros>
       </plugin>
     </gazebo>
diff --git a/panther_description/launch/rviz.launch.py b/panther_description/launch/rviz.launch.py
new file mode 100644
index 00000000..9b4d3455
--- /dev/null
+++ b/panther_description/launch/rviz.launch.py
@@ -0,0 +1,91 @@
+#!/usr/bin/env python3
+
+# Copyright 2020 ros2_control Development Team
+# 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.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import (
+    EnvironmentVariable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression
+)
+from launch_ros.actions import Node, SetParameter
+from launch_ros.substitutions import FindPackageShare
+from nav2_common.launch import ReplaceString
+
+
+def generate_launch_description():
+
+    namespace = LaunchConfiguration("namespace")
+    declare_namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Add namespace to all launched nodes.",
+    )
+
+    rviz_config = LaunchConfiguration("rviz_config")
+    declare_rviz_config_arg = DeclareLaunchArgument(
+        "rviz_config",
+        default_value=PathJoinSubstitution(
+            [
+                FindPackageShare("panther_description"), "rviz", "husarion_ugv.rviz"]
+        ),
+        description="RViz configuration file.",
+    )
+
+    use_rviz = LaunchConfiguration("use_rviz")
+    declare_use_rviz_arg = DeclareLaunchArgument(
+        "use_rviz",
+        default_value="True",
+        description="Run RViz simultaneously.",
+        choices=["True", "true", "False", "false"],
+    )
+
+    use_sim = LaunchConfiguration("use_sim")
+    declare_use_sim_arg = DeclareLaunchArgument(
+        "use_sim",
+        default_value="False",
+        description="Whether simulation is used.",
+        choices=["True", "true", "False", "false"],
+    )
+
+    ns_ext = PythonExpression(["'' if '", namespace, "' else '", namespace, "' + '/'"])
+
+    rviz_config = ReplaceString(
+        source_file=rviz_config,
+        replacements={"<robot_namespace>/": ns_ext, "<robot_namespace>": namespace},
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        namespace=namespace,
+        arguments=["-d", rviz_config],
+        condition=IfCondition(use_rviz),
+    )
+
+    actions = [
+        declare_namespace_arg,
+        declare_rviz_config_arg,
+        declare_use_rviz_arg,
+        declare_use_sim_arg,
+        SetParameter(name="use_sim_time", value=use_sim),
+        rviz_node,
+    ]
+
+    return LaunchDescription(actions)
diff --git a/panther_description/package.xml b/panther_description/package.xml
index 8ebdd01d..6661fb5a 100644
--- a/panther_description/package.xml
+++ b/panther_description/package.xml
@@ -20,8 +20,10 @@
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_ros</exec_depend>
+  <exec_depend>nav2_common</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>ros_components_description</exec_depend>
+  <exec_depend>rviz</exec_depend>
   <exec_depend>xacro</exec_depend>
 
   <export>
diff --git a/panther_description/rviz/husarion_ugv.rviz b/panther_description/rviz/husarion_ugv.rviz
new file mode 100644
index 00000000..744745f6
--- /dev/null
+++ b/panther_description/rviz/husarion_ugv.rviz
@@ -0,0 +1,268 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Splitter Ratio: 0.5
+    Tree Height: 719
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 0.5
+          Cell Size: 1
+          Class: rviz_default_plugins/Grid
+          Color: 160; 160; 164
+          Enabled: true
+          Line Style:
+            Line Width: 0.029999999329447746
+            Value: Lines
+          Name: Grid
+          Normal Cell Count: 0
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Plane: XY
+          Plane Cell Count: 10
+          Reference Frame: <Fixed Frame>
+          Value: true
+      Enabled: true
+      Name: Map
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 1
+          Class: rviz_default_plugins/RobotModel
+          Collision Enabled: false
+          Description File: ""
+          Description Source: Topic
+          Description Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: robot_description
+          Enabled: true
+          Links:
+            All Links Enabled: true
+            Expand Joint Details: false
+            Expand Link Details: false
+            Expand Tree: false
+            Link Tree Style: Links in Alphabetic Order
+            base_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            body_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+              Value: true
+            cover_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            fl_wheel_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+              Value: true
+            fr_wheel_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+              Value: true
+            front_bumper_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            imu_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            lights_channel_1_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            lights_channel_2_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            rear_bumper_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            rl_wheel_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+              Value: true
+            rr_wheel_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+              Value: true
+          Mass Properties:
+            Inertia: false
+            Mass: false
+          Name: RobotModel
+          TF Prefix: "<robot_namespace>"
+          Update Interval: 0
+          Value: true
+          Visual Enabled: true
+        - Class: rviz_default_plugins/TF
+          Enabled: true
+          Frame Timeout: 15
+          Frames:
+            All Enabled: true
+          Marker Scale: 1
+          Name: TF
+          Show Arrows: true
+          Show Axes: true
+          Show Names: false
+          Update Interval: 0
+          Value: true
+        - Angle Tolerance: 0.10000000149011612
+          Class: rviz_default_plugins/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.05000000074505806
+              Color: 255; 25; 0
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.05000000074505806
+              Color: 255; 25; 0
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: true
+          Keep: 1000
+          Name: Odometry
+          Position Tolerance: 0.10000000149011612
+          Shape:
+            Alpha: 1
+            Axes Length: 1
+            Axes Radius: 0.10000000149011612
+            Color: 255; 25; 0
+            Head Length: 0.20000000298023224
+            Head Radius: 0.10000000149011612
+            Shaft Length: 0.5
+            Shaft Radius: 0.05000000074505806
+            Value: Arrow
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: odometry/filtered
+          Value: true
+      Enabled: true
+      Name: Robot
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: <robot_namespace>/odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 5.996953964233398
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 1.012673020362854
+        Y: 0.033135633915662766
+        Z: 0.347855806350708
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5903985500335693
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.3703993558883667
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1920
+  X: 0
+  Y: 27
diff --git a/panther_description/rviz/panther.rviz b/panther_description/rviz/panther.rviz
deleted file mode 100644
index 3fce6812..00000000
--- a/panther_description/rviz/panther.rviz
+++ /dev/null
@@ -1,211 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-      Splitter Ratio: 0.5
-    Tree Height: 719
-  - Class: rviz_common/Selection
-    Name: Selection
-  - Class: rviz_common/Tool Properties
-    Expanded:
-      - /2D Goal Pose1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz_common/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: rviz_common/Time
-    Experimental: false
-    Name: Time
-    SyncMode: 0
-    SyncSource: ""
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz_default_plugins/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Alpha: 1
-      Class: rviz_default_plugins/RobotModel
-      Collision Enabled: false
-      Description File: ""
-      Description Source: Topic
-      Description Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_description
-      Enabled: true
-      Links:
-        All Links Enabled: true
-        Expand Joint Details: false
-        Expand Link Details: false
-        Expand Tree: false
-        Link Tree Style: Links in Alphabetic Order
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        body_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        cover_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        fl_wheel_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        fr_wheel_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        front_bumper_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        front_light_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        imu_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        rear_bumper_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        rear_light_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        rl_wheel_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rr_wheel_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-      Mass Properties:
-        Inertia: false
-        Mass: false
-      Name: RobotModel
-      TF Prefix: ""
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: odom
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz_default_plugins/Interact
-      Hide Inactive Objects: true
-    - Class: rviz_default_plugins/MoveCamera
-    - Class: rviz_default_plugins/Select
-    - Class: rviz_default_plugins/FocusCamera
-    - Class: rviz_default_plugins/Measure
-      Line color: 128; 128; 0
-    - Class: rviz_default_plugins/SetInitialPose
-      Covariance x: 0.25
-      Covariance y: 0.25
-      Covariance yaw: 0.06853891909122467
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /initialpose
-    - Class: rviz_default_plugins/SetGoal
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /goal_pose
-    - Class: rviz_default_plugins/PublishPoint
-      Single click: true
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /clicked_point
-  Transformation:
-    Current:
-      Class: rviz_default_plugins/TF
-  Value: true
-  Views:
-    Current:
-      Class: rviz_default_plugins/Orbit
-      Distance: 10
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
-      Focal Shape Fixed Size: false
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7853981852531433
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 0.7853981852531433
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 1016
-  Hide Left Dock: false
-  Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005de0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: true
-  Width: 1850
-  X: 1990
-  Y: 27
diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro
index 2d520a1c..40918200 100644
--- a/panther_description/urdf/gazebo.urdf.xacro
+++ b/panther_description/urdf/gazebo.urdf.xacro
@@ -37,12 +37,15 @@
         <parameters>${config_file}</parameters>
         <ros>
           <namespace>${namespace}</namespace>
+          <remapping>drive_controller/cmd_vel_unstamped:=cmd_vel</remapping>
+          <remapping>drive_controller/odom:=odometry/wheels</remapping>
+          <remapping>drive_controller/transition_event:=drive_controller/_transition_event</remapping>
           <remapping>gz_ros2_control/e_stop:=hardware/e_stop</remapping>
           <remapping>gz_ros2_control/e_stop_reset:=hardware/e_stop_reset</remapping>
           <remapping>gz_ros2_control/e_stop_trigger:=hardware/e_stop_trigger</remapping>
           <remapping>imu_broadcaster/imu:=imu/data</remapping>
-          <remapping>drive_controller/cmd_vel_unstamped:=cmd_vel</remapping>
-          <remapping>drive_controller/odom:=odometry/wheels</remapping>
+          <remapping>imu_broadcaster/transition_event:=imu_broadcaster/_transition_event</remapping>
+          <remapping>joint_state_broadcaster/transition_event:=joint_state_broadcaster/_transition_event</remapping>
         </ros>
       </plugin>
     </gazebo>

From db610e845a1e558ae86bbb3b1df67ee8f1c93589 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Thu, 31 Oct 2024 11:57:59 +0100
Subject: [PATCH 077/100] pre-commit fix

---
 panther_description/launch/rviz.launch.py | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/panther_description/launch/rviz.launch.py b/panther_description/launch/rviz.launch.py
index 9b4d3455..8f0cb3e2 100644
--- a/panther_description/launch/rviz.launch.py
+++ b/panther_description/launch/rviz.launch.py
@@ -22,7 +22,7 @@
     EnvironmentVariable,
     LaunchConfiguration,
     PathJoinSubstitution,
-    PythonExpression
+    PythonExpression,
 )
 from launch_ros.actions import Node, SetParameter
 from launch_ros.substitutions import FindPackageShare
@@ -42,8 +42,7 @@ def generate_launch_description():
     declare_rviz_config_arg = DeclareLaunchArgument(
         "rviz_config",
         default_value=PathJoinSubstitution(
-            [
-                FindPackageShare("panther_description"), "rviz", "husarion_ugv.rviz"]
+            [FindPackageShare("panther_description"), "rviz", "husarion_ugv.rviz"]
         ),
         description="RViz configuration file.",
     )

From b9420f58ba9fdd60bd02fef3a37541059ce15f4d Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Wed, 6 Nov 2024 11:13:47 +0100
Subject: [PATCH 078/100] Hide visualization

---
 panther_description/rviz/husarion_ugv.rviz | 66 +++-------------------
 1 file changed, 8 insertions(+), 58 deletions(-)

diff --git a/panther_description/rviz/husarion_ugv.rviz b/panther_description/rviz/husarion_ugv.rviz
index 744745f6..8285a047 100644
--- a/panther_description/rviz/husarion_ugv.rviz
+++ b/panther_description/rviz/husarion_ugv.rviz
@@ -3,6 +3,7 @@ Panels:
     Help Height: 78
     Name: Displays
     Property Tree Widget:
+      Expanded: ~
       Splitter Ratio: 0.5
     Tree Height: 719
   - Class: rviz_common/Selection
@@ -68,69 +69,16 @@ Visualization Manager:
             Expand Link Details: false
             Expand Tree: false
             Link Tree Style: Links in Alphabetic Order
-            base_link:
-              Alpha: 1
-              Show Axes: false
-              Show Trail: false
-            body_link:
-              Alpha: 1
-              Show Axes: false
-              Show Trail: false
-              Value: true
-            cover_link:
-              Alpha: 1
-              Show Axes: false
-              Show Trail: false
-            fl_wheel_link:
-              Alpha: 1
-              Show Axes: false
-              Show Trail: false
-              Value: true
-            fr_wheel_link:
-              Alpha: 1
-              Show Axes: false
-              Show Trail: false
-              Value: true
-            front_bumper_link:
-              Alpha: 1
-              Show Axes: false
-              Show Trail: false
-            imu_link:
-              Alpha: 1
-              Show Axes: false
-              Show Trail: false
-            lights_channel_1_link:
-              Alpha: 1
-              Show Axes: false
-              Show Trail: false
-            lights_channel_2_link:
-              Alpha: 1
-              Show Axes: false
-              Show Trail: false
-            rear_bumper_link:
-              Alpha: 1
-              Show Axes: false
-              Show Trail: false
-            rl_wheel_link:
-              Alpha: 1
-              Show Axes: false
-              Show Trail: false
-              Value: true
-            rr_wheel_link:
-              Alpha: 1
-              Show Axes: false
-              Show Trail: false
-              Value: true
           Mass Properties:
             Inertia: false
             Mass: false
           Name: RobotModel
-          TF Prefix: "<robot_namespace>"
+          TF Prefix: <robot_namespace>
           Update Interval: 0
           Value: true
           Visual Enabled: true
         - Class: rviz_default_plugins/TF
-          Enabled: true
+          Enabled: false
           Frame Timeout: 15
           Frames:
             All Enabled: true
@@ -139,8 +87,10 @@ Visualization Manager:
           Show Arrows: true
           Show Axes: true
           Show Names: false
+          Tree:
+            {}
           Update Interval: 0
-          Value: true
+          Value: false
         - Angle Tolerance: 0.10000000149011612
           Class: rviz_default_plugins/Odometry
           Covariance:
@@ -158,7 +108,7 @@ Visualization Manager:
               Scale: 1
               Value: true
             Value: true
-          Enabled: true
+          Enabled: false
           Keep: 1000
           Name: Odometry
           Position Tolerance: 0.10000000149011612
@@ -179,7 +129,7 @@ Visualization Manager:
             History Policy: Keep Last
             Reliability Policy: Reliable
             Value: odometry/filtered
-          Value: true
+          Value: false
       Enabled: true
       Name: Robot
   Enabled: true

From a2a9ee921abf8d2a342dd011e653934052d6f2bd Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Thu, 7 Nov 2024 10:59:40 +0100
Subject: [PATCH 079/100] Move RViz arg

---
 husarion_ugv_gazebo/launch/simulation.launch.py | 11 +++++++++++
 panther_description/launch/rviz.launch.py       | 11 -----------
 2 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/husarion_ugv_gazebo/launch/simulation.launch.py b/husarion_ugv_gazebo/launch/simulation.launch.py
index cf34f986..34da7dc9 100644
--- a/husarion_ugv_gazebo/launch/simulation.launch.py
+++ b/husarion_ugv_gazebo/launch/simulation.launch.py
@@ -16,6 +16,7 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
     EnvironmentVariable,
@@ -45,6 +46,14 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
+    use_rviz = LaunchConfiguration("use_rviz")
+    declare_use_rviz_arg = DeclareLaunchArgument(
+        "use_rviz",
+        default_value="True",
+        description="Run RViz simultaneously.",
+        choices=["True", "true", "False", "false"],
+    )
+
     namespaced_gz_gui = ReplaceString(
         source_file=gz_gui,
         replacements={"{namespace}": namespace},
@@ -69,6 +78,7 @@ def generate_launch_description():
                 ]
             )
         ),
+        condition=IfCondition(use_rviz),
     )
 
     simulate_robots = IncludeLaunchDescription(
@@ -86,6 +96,7 @@ def generate_launch_description():
     actions = [
         declare_gz_gui,
         declare_namespace_arg,
+        declare_use_rviz_arg,
         # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo)
         SetUseSimTime(True),
         gz_sim,
diff --git a/panther_description/launch/rviz.launch.py b/panther_description/launch/rviz.launch.py
index 8f0cb3e2..b194341c 100644
--- a/panther_description/launch/rviz.launch.py
+++ b/panther_description/launch/rviz.launch.py
@@ -17,7 +17,6 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition
 from launch.substitutions import (
     EnvironmentVariable,
     LaunchConfiguration,
@@ -47,14 +46,6 @@ def generate_launch_description():
         description="RViz configuration file.",
     )
 
-    use_rviz = LaunchConfiguration("use_rviz")
-    declare_use_rviz_arg = DeclareLaunchArgument(
-        "use_rviz",
-        default_value="True",
-        description="Run RViz simultaneously.",
-        choices=["True", "true", "False", "false"],
-    )
-
     use_sim = LaunchConfiguration("use_sim")
     declare_use_sim_arg = DeclareLaunchArgument(
         "use_sim",
@@ -75,13 +66,11 @@ def generate_launch_description():
         executable="rviz2",
         namespace=namespace,
         arguments=["-d", rviz_config],
-        condition=IfCondition(use_rviz),
     )
 
     actions = [
         declare_namespace_arg,
         declare_rviz_config_arg,
-        declare_use_rviz_arg,
         declare_use_sim_arg,
         SetParameter(name="use_sim_time", value=use_sim),
         rviz_node,

From 76df718b41d41940f1261406c67a07a7c38dc00f Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Thu, 7 Nov 2024 12:53:45 +0100
Subject: [PATCH 080/100] Update package.xml

---
 panther_description/package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/panther_description/package.xml b/panther_description/package.xml
index 6661fb5a..333fb992 100644
--- a/panther_description/package.xml
+++ b/panther_description/package.xml
@@ -24,6 +24,7 @@
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>ros_components_description</exec_depend>
   <exec_depend>rviz</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">rviz2</exec_depend>
   <exec_depend>xacro</exec_depend>
 
   <export>

From 77a1469b7caae57890be60d6c2818d491f570d37 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Thu, 7 Nov 2024 12:55:09 +0100
Subject: [PATCH 081/100] Update package.xml

---
 panther_description/package.xml | 1 -
 1 file changed, 1 deletion(-)

diff --git a/panther_description/package.xml b/panther_description/package.xml
index 333fb992..6df0f171 100644
--- a/panther_description/package.xml
+++ b/panther_description/package.xml
@@ -23,7 +23,6 @@
   <exec_depend>nav2_common</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>ros_components_description</exec_depend>
-  <exec_depend>rviz</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">rviz2</exec_depend>
   <exec_depend>xacro</exec_depend>
 

From be607f26a377e548c413539c51a95540cbe418da Mon Sep 17 00:00:00 2001
From: Milosz Lagan <milosz.lagan@husarion.com>
Date: Thu, 7 Nov 2024 21:58:48 +0000
Subject: [PATCH 082/100] Generate node parameters from yaml files

---
 husarion_ugv_battery/CMakeLists.txt           | 25 ++++-
 husarion_ugv_battery/README.md                |  4 +
 .../config/battery_parameters.yaml            | 52 +++++++++++
 .../battery_driver_node.hpp                   |  5 +
 .../battery_publisher/battery_publisher.hpp   |  3 +-
 .../dual_battery_publisher.hpp                |  3 +-
 .../single_battery_publisher.hpp              |  2 +-
 husarion_ugv_battery/package.xml              |  1 +
 .../src/battery_driver_node.cpp               | 37 ++++----
 .../battery_publisher/battery_publisher.cpp   | 10 +-
 .../dual_battery_publisher.cpp                |  5 +-
 .../single_battery_publisher.cpp              |  5 +-
 .../test_battery_publisher.cpp                |  5 +-
 .../test_dual_battery_publisher.cpp           |  5 +-
 .../test_single_battery_publisher.cpp         |  4 +-
 husarion_ugv_lights/CMakeLists.txt            | 15 ++-
 husarion_ugv_lights/README.md                 |  5 +-
 .../config/lights_controller_parameters.yaml  | 17 ++++
 .../config/lights_driver_parameters.yaml      | 24 +++++
 .../lights_controller_node.hpp                |  5 +
 .../lights_driver_node.hpp                    |  5 +
 husarion_ugv_lights/package.xml               |  1 +
 .../src/lights_controller_node.cpp            | 14 +--
 .../src/lights_driver_node.cpp                | 15 ++-
 husarion_ugv_manager/CMakeLists.txt           | 21 ++++-
 husarion_ugv_manager/README.md                |  2 +
 .../config/lights_manager_parameters.yaml     | 75 +++++++++++++++
 .../config/safety_manager_parameters.yaml     | 93 +++++++++++++++++++
 .../lights_manager_node.hpp                   |  5 +
 .../safety_manager_node.hpp                   |  5 +
 husarion_ugv_manager/package.xml              |  1 +
 .../src/lights_manager_node.cpp               | 58 +++---------
 .../src/safety_manager_node.cpp               | 62 ++++---------
 33 files changed, 444 insertions(+), 145 deletions(-)
 create mode 100644 husarion_ugv_battery/config/battery_parameters.yaml
 create mode 100644 husarion_ugv_lights/config/lights_controller_parameters.yaml
 create mode 100644 husarion_ugv_lights/config/lights_driver_parameters.yaml
 create mode 100644 husarion_ugv_manager/config/lights_manager_parameters.yaml
 create mode 100644 husarion_ugv_manager/config/safety_manager_parameters.yaml

diff --git a/husarion_ugv_battery/CMakeLists.txt b/husarion_ugv_battery/CMakeLists.txt
index d928de57..a3dc0c8d 100644
--- a/husarion_ugv_battery/CMakeLists.txt
+++ b/husarion_ugv_battery/CMakeLists.txt
@@ -5,8 +5,14 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-set(PACKAGE_DEPENDENCIES ament_cmake diagnostic_updater panther_msgs
-                         husarion_ugv_utils rclcpp sensor_msgs)
+set(PACKAGE_DEPENDENCIES
+    ament_cmake
+    diagnostic_updater
+    generate_parameter_library
+    panther_msgs
+    husarion_ugv_utils
+    rclcpp
+    sensor_msgs)
 
 foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
   find_package(${PACKAGE} REQUIRED)
@@ -25,6 +31,9 @@ add_executable(
   src/battery_publisher/single_battery_publisher.cpp)
 ament_target_dependencies(battery_driver_node ${PACKAGE_DEPENDENCIES})
 
+generate_parameter_library(battery_parameters config/battery_parameters.yaml)
+target_link_libraries(battery_driver_node battery_parameters)
+
 install(TARGETS battery_driver_node DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
@@ -76,6 +85,8 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_battery_publisher
                             ${PACKAGE_DEPENDENCIES})
+  target_link_libraries(${PROJECT_NAME}_test_battery_publisher
+                        battery_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_single_battery_publisher
@@ -89,6 +100,8 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_single_battery_publisher
                             ${PACKAGE_DEPENDENCIES})
+  target_link_libraries(${PROJECT_NAME}_test_single_battery_publisher
+                        battery_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_dual_battery_publisher
@@ -102,6 +115,8 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_dual_battery_publisher
                             ${PACKAGE_DEPENDENCIES})
+  target_link_libraries(${PROJECT_NAME}_test_dual_battery_publisher
+                        battery_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_battery_driver_node_adc_dual
@@ -118,6 +133,8 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_adc_dual
                             ${PACKAGE_DEPENDENCIES})
+  target_link_libraries(${PROJECT_NAME}_test_battery_driver_node_adc_dual
+                        battery_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_battery_driver_node_adc_single
@@ -134,6 +151,8 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_adc_single
                             ${PACKAGE_DEPENDENCIES})
+  target_link_libraries(${PROJECT_NAME}_test_battery_driver_node_adc_single
+                        battery_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_battery_driver_node_roboteq
@@ -150,6 +169,8 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_roboteq
                             ${PACKAGE_DEPENDENCIES})
+  target_link_libraries(${PROJECT_NAME}_test_battery_driver_node_roboteq
+                        battery_parameters)
 
 endif()
 
diff --git a/husarion_ugv_battery/README.md b/husarion_ugv_battery/README.md
index a6fcc35f..e952f05d 100644
--- a/husarion_ugv_battery/README.md
+++ b/husarion_ugv_battery/README.md
@@ -8,6 +8,10 @@ This package contains:
 
 - `battery.launch.py`: Responsible for activating battery node, which dealing with reading and publishing battery data.
 
+## Configuration Files
+
+- [`battery_parameters.yaml`](./config/battery_parameters.yaml): Defines parameters for `battery_driver_node`.
+
 ## ROS Nodes
 
 ### battery_driver_node
diff --git a/husarion_ugv_battery/config/battery_parameters.yaml b/husarion_ugv_battery/config/battery_parameters.yaml
new file mode 100644
index 00000000..ab09541c
--- /dev/null
+++ b/husarion_ugv_battery/config/battery_parameters.yaml
@@ -0,0 +1,52 @@
+battery:
+  adc:
+    device0:
+      type: string
+      default_value: "/sys/bus/iio/devices/iio:device0"
+      description: "Internal ADC0 IIO device."
+      validation: { not_empty<> }
+
+    device1:
+      type: string
+      default_value: "/sys/bus/iio/devices/iio:device1"
+      description: "Internal ADC1 IIO device."
+      validation: { not_empty<> }
+
+    ma_window_len:
+      charge:
+        type: int
+        default_value: 10
+        description: "Window length of a moving average, used to smooth out battery charge readings."
+        validation: { gt<>: 0 }
+
+      temp:
+        type: int
+        default_value: 10
+        description: "Window length of a moving average, used to smooth out battery temperature readings."
+        validation: { gt<>: 0 }
+
+  ma_window_len:
+    voltage:
+      type: int
+      default_value: 10
+      description: "Window length of a moving average, used to smooth out battery voltage readings."
+      validation: { gt<>: 0 }
+
+    current:
+      type: int
+      default_value: 10
+      description: "Window length of a moving average, used to smooth out battery current readings."
+      validation: { gt<>: 0 }
+
+  roboteq:
+    driver_state_timeout:
+      type: double
+      default_value: 0.2
+      description: "Timeout in seconds after which driver state messages will be considered old. Used as a fallback when ADC data is not available."
+      validation: { gt<>: 0.0 }
+
+  battery_timeout:
+    type: double
+    default_value: 1.0
+    description: "Timeout in seconds. If the node fails to read battery data exceeding this duration, the node will publish an unknown battery state."
+    validation: { gt<>: 0.0 }
diff --git a/husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp
index 1bba8b99..ca113540 100644
--- a/husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp
+++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp
@@ -23,6 +23,8 @@
 
 #include "panther_msgs/msg/robot_driver_state.hpp"
 
+#include "battery_parameters.hpp"
+
 #include "husarion_ugv_battery/adc_data_reader.hpp"
 #include "husarion_ugv_battery/battery/battery.hpp"
 #include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp"
@@ -54,6 +56,9 @@ class BatteryDriverNode : public rclcpp::Node
   std::shared_ptr<Battery> battery_2_;
   std::shared_ptr<BatteryPublisher> battery_publisher_;
 
+  std::shared_ptr<battery::ParamListener> param_listener_;
+  battery::Params params_;
+
   rclcpp::Subscription<RobotDriverStateMsg>::SharedPtr driver_state_sub_;
   rclcpp::TimerBase::SharedPtr battery_pub_timer_;
 
diff --git a/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/battery_publisher.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/battery_publisher.hpp
index 9258b9a5..29e52609 100644
--- a/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/battery_publisher.hpp
+++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/battery_publisher.hpp
@@ -37,7 +37,8 @@ class BatteryPublisher
 public:
   BatteryPublisher(
     const rclcpp::Node::SharedPtr & node,
-    const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater);
+    const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater,
+    const double battery_timeout);
 
   ~BatteryPublisher() {}
 
diff --git a/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp
index 738d7766..4200c621 100644
--- a/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp
+++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp
@@ -32,7 +32,8 @@ class DualBatteryPublisher : public BatteryPublisher
   DualBatteryPublisher(
     const rclcpp::Node::SharedPtr & node,
     const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater,
-    const std::shared_ptr<Battery> & battery_1, const std::shared_ptr<Battery> & battery_2);
+    const double battery_timeout, const std::shared_ptr<Battery> & battery_1,
+    const std::shared_ptr<Battery> & battery_2);
 
   ~DualBatteryPublisher() {}
 
diff --git a/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp
index f1e65ecf..1b68e2e9 100644
--- a/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp
+++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp
@@ -31,7 +31,7 @@ class SingleBatteryPublisher : public BatteryPublisher
   SingleBatteryPublisher(
     const rclcpp::Node::SharedPtr & node,
     const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater,
-    const std::shared_ptr<Battery> & battery);
+    const double battery_timeout, const std::shared_ptr<Battery> & battery);
 
   ~SingleBatteryPublisher() {}
 
diff --git a/husarion_ugv_battery/package.xml b/husarion_ugv_battery/package.xml
index 70f025d7..512b2323 100644
--- a/husarion_ugv_battery/package.xml
+++ b/husarion_ugv_battery/package.xml
@@ -17,6 +17,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>diagnostic_updater</depend>
+  <depend>generate_parameter_library</depend>
   <depend>husarion_ugv_utils</depend>
   <depend>panther_msgs</depend>
   <depend>rclcpp</depend>
diff --git a/husarion_ugv_battery/src/battery_driver_node.cpp b/husarion_ugv_battery/src/battery_driver_node.cpp
index 9ec16665..db6a063b 100644
--- a/husarion_ugv_battery/src/battery_driver_node.cpp
+++ b/husarion_ugv_battery/src/battery_driver_node.cpp
@@ -41,8 +41,9 @@ BatteryDriverNode::BatteryDriverNode(
 {
   RCLCPP_INFO(this->get_logger(), "Constructing node.");
 
-  this->declare_parameter<int>("ma_window_len/voltage", 10);
-  this->declare_parameter<int>("ma_window_len/current", 10);
+  this->param_listener_ =
+    std::make_shared<battery::ParamListener>(this->get_node_parameters_interface());
+  this->params_ = this->param_listener_->get_params();
 
   // Running at 10 Hz
   battery_pub_timer_ = this->create_wall_timer(
@@ -75,22 +76,17 @@ void BatteryDriverNode::InitializeWithADCBattery()
 {
   RCLCPP_DEBUG(this->get_logger(), "Initializing with ADC data.");
 
-  this->declare_parameter<std::string>("adc/device0", "/sys/bus/iio/devices/iio:device0");
-  this->declare_parameter<std::string>("adc/device1", "/sys/bus/iio/devices/iio:device1");
-  this->declare_parameter<int>("adc/ma_window_len/temp", 10);
-  this->declare_parameter<int>("adc/ma_window_len/charge", 10);
-
-  const std::string adc0_device_path = this->get_parameter("adc/device0").as_string();
-  const std::string adc1_device_path = this->get_parameter("adc/device1").as_string();
+  const std::string adc0_device_path = this->params_.adc.device0;
+  const std::string adc1_device_path = this->params_.adc.device1;
 
   adc0_reader_ = std::make_shared<ADCDataReader>(adc0_device_path);
   adc1_reader_ = std::make_shared<ADCDataReader>(adc1_device_path);
 
   const ADCBatteryParams battery_params = {
-    static_cast<std::size_t>(this->get_parameter("ma_window_len/voltage").as_int()),
-    static_cast<std::size_t>(this->get_parameter("ma_window_len/current").as_int()),
-    static_cast<std::size_t>(this->get_parameter("adc/ma_window_len/temp").as_int()),
-    static_cast<std::size_t>(this->get_parameter("adc/ma_window_len/charge").as_int()),
+    static_cast<std::size_t>(this->params_.ma_window_len.voltage),
+    static_cast<std::size_t>(this->params_.ma_window_len.current),
+    static_cast<std::size_t>(this->params_.adc.ma_window_len.temp),
+    static_cast<std::size_t>(this->params_.adc.ma_window_len.charge),
   };
 
   battery_2_ = std::make_shared<ADCBattery>(
@@ -106,7 +102,8 @@ void BatteryDriverNode::InitializeWithADCBattery()
       std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 1, 0),
       std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 3, 0), battery_params);
     battery_publisher_ = std::make_shared<DualBatteryPublisher>(
-      this->shared_from_this(), diagnostic_updater_, battery_1_, battery_2_);
+      this->shared_from_this(), diagnostic_updater_, params_.battery_timeout, battery_1_,
+      battery_2_);
   } else {
     battery_2_.reset();
     battery_1_ = std::make_shared<ADCBattery>(
@@ -118,7 +115,7 @@ void BatteryDriverNode::InitializeWithADCBattery()
       std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 1, 0),
       std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 3, 0), battery_params);
     battery_publisher_ = std::make_shared<SingleBatteryPublisher>(
-      this->shared_from_this(), diagnostic_updater_, battery_1_);
+      this->shared_from_this(), diagnostic_updater_, params_.battery_timeout, battery_1_);
   }
 
   RCLCPP_INFO(this->get_logger(), "Initialized battery driver using ADC data.");
@@ -128,12 +125,10 @@ void BatteryDriverNode::InitializeWithRoboteqBattery()
 {
   RCLCPP_DEBUG(this->get_logger(), "Initializing with Roboteq data.");
 
-  this->declare_parameter<float>("roboteq/driver_state_timeout", 0.2);
-
   const RoboteqBatteryParams battery_params = {
-    static_cast<float>(this->get_parameter("roboteq/driver_state_timeout").as_double()),
-    static_cast<std::size_t>(this->get_parameter("ma_window_len/voltage").as_int()),
-    static_cast<std::size_t>(this->get_parameter("ma_window_len/current").as_int()),
+    static_cast<float>(this->params_.roboteq.driver_state_timeout),
+    static_cast<std::size_t>(this->params_.ma_window_len.voltage),
+    static_cast<std::size_t>(this->params_.ma_window_len.current),
   };
 
   driver_state_sub_ = this->create_subscription<RobotDriverStateMsg>(
@@ -143,7 +138,7 @@ void BatteryDriverNode::InitializeWithRoboteqBattery()
   battery_1_ = std::make_shared<RoboteqBattery>([&]() { return driver_state_; }, battery_params);
 
   battery_publisher_ = std::make_shared<SingleBatteryPublisher>(
-    this->shared_from_this(), diagnostic_updater_, battery_1_);
+    this->shared_from_this(), diagnostic_updater_, this->params_.battery_timeout, battery_1_);
 
   RCLCPP_INFO(this->get_logger(), "Initialized battery driver using motor controllers data.");
 }
diff --git a/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp b/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp
index e37fcbfe..6f2ef870 100644
--- a/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp
+++ b/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp
@@ -26,12 +26,12 @@ namespace husarion_ugv_battery
 
 BatteryPublisher::BatteryPublisher(
   const rclcpp::Node::SharedPtr & node,
-  const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater)
-: node_(std::move(node)), diagnostic_updater_(std::move(diagnostic_updater))
+  const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater,
+  const double battery_timeout)
+: node_(std::move(node)),
+  diagnostic_updater_(std::move(diagnostic_updater)),
+  battery_timeout_(battery_timeout)
 {
-  node->declare_parameter<float>("battery_timeout", 1.0);
-  battery_timeout_ = node->get_parameter("battery_timeout").as_double();
-
   charger_connected_ = false;
   last_battery_info_time_ = rclcpp::Time(std::int64_t(0), RCL_ROS_TIME);
 
diff --git a/husarion_ugv_battery/src/battery_publisher/dual_battery_publisher.cpp b/husarion_ugv_battery/src/battery_publisher/dual_battery_publisher.cpp
index f474b1fe..1e345280 100644
--- a/husarion_ugv_battery/src/battery_publisher/dual_battery_publisher.cpp
+++ b/husarion_ugv_battery/src/battery_publisher/dual_battery_publisher.cpp
@@ -34,8 +34,9 @@ namespace husarion_ugv_battery
 DualBatteryPublisher::DualBatteryPublisher(
   const rclcpp::Node::SharedPtr & node,
   const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater,
-  const std::shared_ptr<Battery> & battery_1, const std::shared_ptr<Battery> & battery_2)
-: BatteryPublisher(std::move(node), std::move(diagnostic_updater)),
+  const double battery_timeout, const std::shared_ptr<Battery> & battery_1,
+  const std::shared_ptr<Battery> & battery_2)
+: BatteryPublisher(std::move(node), std::move(diagnostic_updater), battery_timeout),
   battery_1_(std::move(battery_1)),
   battery_2_(std::move(battery_2))
 {
diff --git a/husarion_ugv_battery/src/battery_publisher/single_battery_publisher.cpp b/husarion_ugv_battery/src/battery_publisher/single_battery_publisher.cpp
index 92e4d003..4fff8f68 100644
--- a/husarion_ugv_battery/src/battery_publisher/single_battery_publisher.cpp
+++ b/husarion_ugv_battery/src/battery_publisher/single_battery_publisher.cpp
@@ -32,8 +32,9 @@ namespace husarion_ugv_battery
 SingleBatteryPublisher::SingleBatteryPublisher(
   const rclcpp::Node::SharedPtr & node,
   const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater,
-  const std::shared_ptr<Battery> & battery)
-: BatteryPublisher(std::move(node), std::move(diagnostic_updater)), battery_(std::move(battery))
+  const double battery_timeout, const std::shared_ptr<Battery> & battery)
+: BatteryPublisher(std::move(node), std::move(diagnostic_updater), battery_timeout),
+  battery_(std::move(battery))
 {
   battery_pub_ = node->create_publisher<BatteryStateMsg>("battery/battery_status", 5);
   battery_1_pub_ = node->create_publisher<BatteryStateMsg>("_battery/battery_1_status_raw", 5);
diff --git a/husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp b/husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp
index f5924c73..ff2fe64a 100644
--- a/husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp
+++ b/husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp
@@ -35,7 +35,7 @@ class BatteryPublisherWrapper : public husarion_ugv_battery::BatteryPublisher
   BatteryPublisherWrapper(
     const rclcpp::Node::SharedPtr & node,
     std::shared_ptr<diagnostic_updater::Updater> diagnostic_updater)
-  : husarion_ugv_battery::BatteryPublisher(node, diagnostic_updater)
+  : husarion_ugv_battery::BatteryPublisher(node, diagnostic_updater, kBatteryTimeout)
   {
   }
 
@@ -62,6 +62,9 @@ class BatteryPublisherWrapper : public husarion_ugv_battery::BatteryPublisher
   {
     status.summary(0, "");  // Avoid unused parameter compiler warning
   };
+
+protected:
+  static constexpr double kBatteryTimeout = 0.2;
 };
 
 class TestBatteryPublisher : public testing::Test
diff --git a/husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp b/husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp
index 74bcb358..b14ecf63 100644
--- a/husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp
+++ b/husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp
@@ -40,7 +40,7 @@ class DualBatteryPublisherWrapper : public husarion_ugv_battery::DualBatteryPubl
     std::shared_ptr<diagnostic_updater::Updater> diagnostic_updater,
     std::shared_ptr<husarion_ugv_battery::Battery> & battery_1,
     std::shared_ptr<husarion_ugv_battery::Battery> & battery_2)
-  : DualBatteryPublisher(node, diagnostic_updater, battery_1, battery_2)
+  : DualBatteryPublisher(node, diagnostic_updater, kBatteryTimeout, battery_1, battery_2)
   {
   }
 
@@ -71,6 +71,9 @@ class DualBatteryPublisherWrapper : public husarion_ugv_battery::DualBatteryPubl
     return DualBatteryPublisher::MergeChargingStatusMsgs(
       charging_status_msg_1, charging_status_msg_2);
   }
+
+private:
+  static constexpr double kBatteryTimeout = 0.2;
 };
 
 class TestDualBatteryPublisher : public testing::Test
diff --git a/husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp b/husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp
index 3d98a9a7..2afd2a37 100644
--- a/husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp
+++ b/husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp
@@ -44,6 +44,8 @@ class TestSingleBatteryPublisher : public testing::Test
   std::shared_ptr<husarion_ugv_battery::BatteryPublisher> battery_publisher_;
   BatteryStateMsg::SharedPtr battery_state_;
   BatteryStateMsg::SharedPtr battery_1_state_;
+
+  static constexpr double kBatteryTimeout = 0.2f;
 };
 
 TestSingleBatteryPublisher::TestSingleBatteryPublisher()
@@ -62,7 +64,7 @@ TestSingleBatteryPublisher::TestSingleBatteryPublisher()
     "/_battery/battery_1_status_raw", 10,
     [&](const BatteryStateMsg::SharedPtr msg) { battery_1_state_ = msg; });
   battery_publisher_ = std::make_shared<husarion_ugv_battery::SingleBatteryPublisher>(
-    node_, diagnostic_updater_, battery_);
+    node_, diagnostic_updater_, kBatteryTimeout, battery_);
 }
 
 TEST_F(TestSingleBatteryPublisher, CorrectTopicPublished)
diff --git a/husarion_ugv_lights/CMakeLists.txt b/husarion_ugv_lights/CMakeLists.txt
index 3fc9de63..2631fc5e 100644
--- a/husarion_ugv_lights/CMakeLists.txt
+++ b/husarion_ugv_lights/CMakeLists.txt
@@ -8,6 +8,7 @@ endif()
 set(PACKAGE_DEPENDENCIES
     ament_cmake
     diagnostic_updater
+    generate_parameter_library
     image_transport
     panther_msgs
     husarion_ugv_utils
@@ -43,6 +44,10 @@ ament_target_dependencies(
   sensor_msgs
   std_srvs)
 
+generate_parameter_library(lights_driver_parameters
+                           config/lights_driver_parameters.yaml)
+target_link_libraries(lights_driver_node_component lights_driver_parameters)
+
 add_library(
   lights_controller_node_component SHARED
   src/lights_controller_node.cpp src/led_components/led_segment.cpp
@@ -56,7 +61,11 @@ ament_target_dependencies(
   rclcpp
   rclcpp_components
   sensor_msgs)
-target_link_libraries(lights_controller_node_component yaml-cpp)
+
+generate_parameter_library(lights_controller_parameters
+                           config/lights_controller_parameters.yaml)
+target_link_libraries(lights_controller_node_component yaml-cpp
+                      lights_controller_parameters)
 
 rclcpp_components_register_node(
   lights_driver_node_component PLUGIN "husarion_ugv_lights::LightsDriverNode"
@@ -68,7 +77,7 @@ rclcpp_components_register_node(
 
 ament_export_targets(export_lights_driver_node_component)
 install(
-  TARGETS lights_driver_node_component
+  TARGETS lights_driver_node_component lights_driver_parameters
   EXPORT export_lights_driver_node_component
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -76,7 +85,7 @@ install(
 
 ament_export_targets(export_lights_controller_node_component)
 install(
-  TARGETS lights_controller_node_component
+  TARGETS lights_controller_node_component lights_controller_parameters
   EXPORT export_lights_controller_node_component
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
diff --git a/husarion_ugv_lights/README.md b/husarion_ugv_lights/README.md
index 50a8c4e6..f53bf287 100644
--- a/husarion_ugv_lights/README.md
+++ b/husarion_ugv_lights/README.md
@@ -12,6 +12,8 @@ This package contains:
 
 - [`{robot_model}_animations.yaml`](./config): Defines and describes the appearance and parameters of the animations for specific robot.
 - [`{robot_model}_driver.yaml`](./config): Defines and describes specific hardware configuration for specific robot.
+- [`lights_controller_parameters.yaml`](./config/lights_controller_parameters.yaml): Defines parameters for `lights_controller_node`.
+- [`lights_driver_parameters.yaml`](./config/lights_driver_parameters.yaml): Defines parameters for `lights_driver_node`.
 
 ## ROS Nodes
 
@@ -59,4 +61,5 @@ This node is of type rclcpp_components is responsible for displaying frames on t
 
 - `~frame_timeout` [*float*, default: **0.1**]: Time in **[s]** after which an incoming frame will be considered too old.
 - `~global_brightness` [*float*, default: **1.0**]: LED global brightness. The range between **[0.0, 1.0]**.
-- `~num_led` [*int*, default: **46**]: Number of LEDs in a single bumper.
+- `~channel_1_num_led` [*int*, default: **46**]: Number of LEDs in the first bumper.
+- `~channel_2_num_led` [*int*, default: **46**]: Number of LEDs in the second bumper.
diff --git a/husarion_ugv_lights/config/lights_controller_parameters.yaml b/husarion_ugv_lights/config/lights_controller_parameters.yaml
new file mode 100644
index 00000000..c4b90ab0
--- /dev/null
+++ b/husarion_ugv_lights/config/lights_controller_parameters.yaml
@@ -0,0 +1,17 @@
+lights_controller:
+  animations_config_path:
+    type: string
+    default_value: ""
+    description: Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations.
+    validation: { not_empty<> }
+
+  controller_frequency:
+    type: double
+    default_value: 50.0
+    description: Frequency in Hz at which the lights controller node will process animations.
+    validation: { gt<>: 0.0 }
+
+  user_led_animations_path:
+    type: string
+    default_value: ""
+    description: Path to a YAML file with a description of the user defined animations.
diff --git a/husarion_ugv_lights/config/lights_driver_parameters.yaml b/husarion_ugv_lights/config/lights_driver_parameters.yaml
new file mode 100644
index 00000000..87e89030
--- /dev/null
+++ b/husarion_ugv_lights/config/lights_driver_parameters.yaml
@@ -0,0 +1,24 @@
+lights_driver:
+  frame_timeout:
+    type: double
+    default_value: 0.1
+    description: Time in seconds after which an incoming frame will be considered too old.
+    validation: { gt<>: 0.0 }
+
+  global_brightness:
+    type: double
+    default_value: 1.0
+    description: LED global brightness. The range should be between [0.0, 1.0].
+    validation: { bounds<>: [0.0, 1.0] }
+
+  channel_1_num_led:
+    type: int
+    default_value: 46
+    description: Number of LEDs in the first bumper.
+    validation: { gt<>: 0 }
+
+  channel_2_num_led:
+    type: int
+    default_value: 46
+    description: Number of LEDs in the second bumper.
+    validation: { gt<>: 0 }
diff --git a/husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp
index fc7b61cc..fd01c3ab 100644
--- a/husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp
+++ b/husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp
@@ -27,6 +27,8 @@
 
 #include "panther_msgs/srv/set_led_animation.hpp"
 
+#include "lights_controller_parameters.hpp"
+
 #include "husarion_ugv_lights/animation/animation.hpp"
 #include "husarion_ugv_lights/led_components/led_animations_queue.hpp"
 #include "husarion_ugv_lights/led_components/segment_converter.hpp"
@@ -148,6 +150,9 @@ class LightsControllerNode : public rclcpp::Node
   std::unordered_map<std::size_t, LEDAnimationDescription> animations_descriptions_;
   std::shared_ptr<SegmentConverter> segment_converter_;
 
+  std::shared_ptr<lights_controller::ParamListener> param_listener_;
+  lights_controller::Params params_;
+
   rclcpp::Service<SetLEDAnimationSrv>::SharedPtr set_led_animation_server_;
   rclcpp::TimerBase::SharedPtr controller_timer_;
 
diff --git a/husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp
index 0a5de4ae..0f015c6c 100644
--- a/husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp
+++ b/husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp
@@ -26,6 +26,8 @@
 
 #include "panther_msgs/srv/set_led_brightness.hpp"
 
+#include "lights_driver_parameters.hpp"
+
 #include "husarion_ugv_lights/apa102.hpp"
 
 namespace husarion_ugv_lights
@@ -120,6 +122,9 @@ class LightsDriverNode : public rclcpp::Node
   APA102Interface::SharedPtr channel_1_;
   APA102Interface::SharedPtr channel_2_;
 
+  std::shared_ptr<lights_driver::ParamListener> param_listener_;
+  lights_driver::Params params_;
+
   rclcpp::TimerBase::SharedPtr initialization_timer_;
 
   rclcpp::Client<SetBoolSrv>::SharedPtr enable_led_control_client_;
diff --git a/husarion_ugv_lights/package.xml b/husarion_ugv_lights/package.xml
index 8e68c10c..116e40c4 100644
--- a/husarion_ugv_lights/package.xml
+++ b/husarion_ugv_lights/package.xml
@@ -17,6 +17,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>diagnostic_updater</depend>
+  <depend>generate_parameter_library</depend>
   <depend>husarion_ugv_utils</depend>
   <depend>image_transport</depend>
   <depend>libpng-dev</depend>
diff --git a/husarion_ugv_lights/src/lights_controller_node.cpp b/husarion_ugv_lights/src/lights_controller_node.cpp
index b54cf25d..e1d51224 100644
--- a/husarion_ugv_lights/src/lights_controller_node.cpp
+++ b/husarion_ugv_lights/src/lights_controller_node.cpp
@@ -31,6 +31,8 @@
 
 #include "panther_msgs/srv/set_led_animation.hpp"
 
+#include "lights_controller_parameters.hpp"
+
 #include "husarion_ugv_lights/led_components/led_animations_queue.hpp"
 #include "husarion_ugv_lights/led_components/led_panel.hpp"
 #include "husarion_ugv_lights/led_components/led_segment.hpp"
@@ -48,13 +50,13 @@ LightsControllerNode::LightsControllerNode(const rclcpp::NodeOptions & options)
 
   using namespace std::placeholders;
 
-  this->declare_parameter<std::string>("animations_config_path");
-  this->declare_parameter<std::string>("user_led_animations_path", "");
-  this->declare_parameter<float>("controller_freq", 50.0);
+  this->param_listener_ =
+    std::make_shared<lights_controller::ParamListener>(this->get_node_parameters_interface());
+  this->params_ = this->param_listener_->get_params();
 
-  const auto animations_config_path = this->get_parameter("animations_config_path").as_string();
-  const auto user_led_animations_path = this->get_parameter("user_led_animations_path").as_string();
-  const float controller_freq = this->get_parameter("controller_freq").as_double();
+  const auto animations_config_path = this->params_.animations_config_path;
+  const auto user_led_animations_path = this->params_.user_led_animations_path;
+  const float controller_freq = this->params_.controller_frequency;
 
   YAML::Node led_config_desc = YAML::LoadFile(animations_config_path);
 
diff --git a/husarion_ugv_lights/src/lights_driver_node.cpp b/husarion_ugv_lights/src/lights_driver_node.cpp
index 62e89236..ad502100 100644
--- a/husarion_ugv_lights/src/lights_driver_node.cpp
+++ b/husarion_ugv_lights/src/lights_driver_node.cpp
@@ -52,16 +52,15 @@ LightsDriverNode::LightsDriverNode(const rclcpp::NodeOptions & options)
 
   rclcpp::on_shutdown(std::bind(&LightsDriverNode::OnShutdown, this));
 
-  this->declare_parameter<double>("global_brightness", 1.0);
-  this->declare_parameter<double>("frame_timeout", 0.1);
-  this->declare_parameter<int>("channel_1_num_led", 46);
-  this->declare_parameter<int>("channel_2_num_led", 46);
+  this->param_listener_ =
+    std::make_shared<lights_driver::ParamListener>(this->get_node_parameters_interface());
+  this->params_ = this->param_listener_->get_params();
 
-  frame_timeout_ = this->get_parameter("frame_timeout").as_double();
-  channel_1_num_led_ = this->get_parameter("channel_1_num_led").as_int();
-  channel_2_num_led_ = this->get_parameter("channel_2_num_led").as_int();
+  frame_timeout_ = this->params_.frame_timeout;
+  channel_1_num_led_ = this->params_.channel_1_num_led;
+  channel_2_num_led_ = this->params_.channel_2_num_led;
 
-  const float global_brightness = this->get_parameter("global_brightness").as_double();
+  const float global_brightness = this->params_.global_brightness;
   channel_1_->SetGlobalBrightness(global_brightness);
   channel_2_->SetGlobalBrightness(global_brightness);
 
diff --git a/husarion_ugv_manager/CMakeLists.txt b/husarion_ugv_manager/CMakeLists.txt
index 29286611..82f9b528 100644
--- a/husarion_ugv_manager/CMakeLists.txt
+++ b/husarion_ugv_manager/CMakeLists.txt
@@ -10,6 +10,7 @@ set(PACKAGE_DEPENDENCIES
     ament_index_cpp
     behaviortree_cpp
     behaviortree_ros2
+    generate_parameter_library
     libssh
     panther_msgs
     husarion_ugv_utils
@@ -71,7 +72,11 @@ ament_target_dependencies(
   rclcpp
   sensor_msgs
   std_msgs)
-target_link_libraries(safety_manager_node ${plugin_libs})
+
+generate_parameter_library(safety_manager_parameters
+                           config/safety_manager_parameters.yaml)
+target_link_libraries(safety_manager_node ${plugin_libs}
+                      safety_manager_parameters)
 
 add_executable(
   lights_manager_node src/lights_manager_node_main.cpp
@@ -84,7 +89,11 @@ ament_target_dependencies(
   rclcpp
   sensor_msgs
   std_msgs)
-target_link_libraries(lights_manager_node ${plugin_libs})
+
+generate_parameter_library(lights_manager_parameters
+                           config/lights_manager_parameters.yaml)
+target_link_libraries(lights_manager_node ${plugin_libs}
+                      lights_manager_parameters)
 
 install(TARGETS ${plugin_libs} DESTINATION lib)
 
@@ -197,6 +206,8 @@ if(BUILD_TESTING)
     rclcpp
     sensor_msgs
     std_msgs)
+  target_link_libraries(${PROJECT_NAME}_test_lights_manager_node
+                        lights_manager_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_lights_behavior_tree
@@ -215,6 +226,8 @@ if(BUILD_TESTING)
     rclcpp
     sensor_msgs
     std_msgs)
+  target_link_libraries(${PROJECT_NAME}_test_lights_behavior_tree
+                        lights_manager_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_safety_manager_node test/test_safety_manager_node.cpp
@@ -232,6 +245,8 @@ if(BUILD_TESTING)
     rclcpp
     sensor_msgs
     std_msgs)
+  target_link_libraries(${PROJECT_NAME}_test_safety_manager_node
+                        safety_manager_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_safety_behavior_tree
@@ -251,6 +266,8 @@ if(BUILD_TESTING)
     sensor_msgs
     std_msgs
     std_srvs)
+  target_link_libraries(${PROJECT_NAME}_test_safety_behavior_tree
+                        safety_manager_parameters)
 endif()
 
 ament_package()
diff --git a/husarion_ugv_manager/README.md b/husarion_ugv_manager/README.md
index b8a9795f..f43f5657 100644
--- a/husarion_ugv_manager/README.md
+++ b/husarion_ugv_manager/README.md
@@ -15,7 +15,9 @@ This package contains:
 - [`SafetyBT.btproj`](./behavior_trees/SafetyBT.btproj): BehaviorTree project for managing Panther safety protocols.
 - [`safety.xml`](./behavior_trees/safety.xml): BehaviorTree for monitoring and managing dangerous situations.
 - [`shutdown.xml`](./behavior_trees/shutdown.xml): BehaviorTree for initiating shutdown procedures.
+- [`lights_manager_parameters.yaml`](./config/lights_manager_parameters.yaml): Defines parameters for the `lights_manager` node.
 - [`lights_manager.yaml`](./config/lights_manager.yaml): Contains parameters for the `lights_manager` node.
+- [`safety_manager_parameters.yaml`](./config/safety_manager_parameters.yaml): Defines parameters for the `safety_manager` node.
 - [`safety_manager.yaml`](./config/safety_manager.yaml): Contains parameters for the `safety_manager` node.
 - [`shutdown_hosts.yaml`](./config/shutdown_hosts.yaml): List with all hosts to request shutdown.
 
diff --git a/husarion_ugv_manager/config/lights_manager_parameters.yaml b/husarion_ugv_manager/config/lights_manager_parameters.yaml
new file mode 100644
index 00000000..e60a404c
--- /dev/null
+++ b/husarion_ugv_manager/config/lights_manager_parameters.yaml
@@ -0,0 +1,75 @@
+lights_manager:
+  battery:
+    charging_anim_step:
+      type: double
+      default_value: 0.1
+      description: This parameter defines the minimum change in battery percentage required to trigger an update in the battery charging animation.
+      validation: { gt<>: 0.0 }
+
+    anim_period:
+      critical:
+        type: double
+        default_value: 15.0
+        description: Time in seconds to wait before repeating animation, indicating a critical Battery state.
+        validation: { gt<>: 0.0 }
+
+      low:
+        type: double
+        default_value: 30.0
+        description: Time in seconds to wait before repeating the animation, indicating a low Battery state.
+        validation: { gt<>: 0.0 }
+
+    percent:
+      window_len:
+        type: int
+        default_value: 6
+        description: Moving average window length used to smooth out Battery percentage readings.
+        validation: { gt<>: 0 }
+
+      threshold:
+        critical:
+          type: double
+          default_value: 0.1
+          description: If the Battery percentage drops below this value, an animation indicating a critical Battery state will start being displayed.
+          validation: { bounds<>: [0.0, 1.0] }
+
+        low:
+          type: double
+          default_value: 0.4
+          description: If the Battery percentage drops below this value, the animation indicating a low Battery state will start being displayed.
+          validation: { bounds<>: [0.0, 1.0] }
+
+  bt_project_path:
+    type: string
+    default_value: ""
+    description: Path to a BehaviorTree project.
+    validation: { not_empty<> }
+
+  plugin_libs:
+    type: string_array
+    default_value: []
+    description: List with names of plugins that are used in the BT project.
+
+  ros_communication_timeout:
+    availability:
+      type: double
+      default_value: 1.0
+      description: Timeout in seconds to wait for a service/action while initializing BT node.
+      validation: { gt<>: 0.0 }
+
+    response:
+      type: double
+      default_value: 1.0
+      description: Timeout in seconds to receive a service/action response after call.
+      validation: { gt<>: 0.0 }
+
+  ros_plugin_libs:
+    type: string_array
+    default_value: []
+    description: List with names of ROS plugins that are used in a BT project.
+
+  timer_frequency:
+    type: double
+    default_value: 10.0
+    description: Frequency in Hz at which lights tree will be ticked.
+    validation: { gt<>: 0.0 }
diff --git a/husarion_ugv_manager/config/safety_manager_parameters.yaml b/husarion_ugv_manager/config/safety_manager_parameters.yaml
new file mode 100644
index 00000000..4010ee12
--- /dev/null
+++ b/husarion_ugv_manager/config/safety_manager_parameters.yaml
@@ -0,0 +1,93 @@
+safety_manager:
+  battery:
+    temp:
+      window_len:
+        type: int
+        default_value: 6
+        description: Moving average window length used to smooth out temperature readings of the battery.
+        validation: { gt<>: 0 }
+
+  bt_project_path:
+    type: string
+    default_value: ""
+    description: Path to a BehaviorTree project.
+    validation: { not_empty<> }
+
+  cpu:
+    temp:
+      fan_off:
+        type: double
+        default_value: 60.0
+        description: Temperature in degrees Celsius of the Built-in Computer's CPU, below which the fan is turned off.
+        validation: { bounds<>: [-20.0, 100.0] }
+
+      fan_on:
+        type: double
+        default_value: 70.0
+        description: Temperature in degrees Celsius of the Built-in Computer's CPU, above which the fan is turned on.
+        validation: { bounds<>: [-20.0, 100.0] }
+
+      window_len:
+        type: int
+        default_value: 6
+        description: Moving average window length used to smooth out temperature readings of the Built-in Computer's CPU.
+        validation: { gt<>: 0 }
+
+  driver:
+    temp:
+      fan_off:
+        type: double
+        default_value: 35.0
+        description: Temperature in degrees Celsius of any drivers below which the fan is turned off.
+        validation: { bounds<>: [-20.0, 100.0] }
+
+      fan_on:
+        type: double
+        default_value: 45.0
+        description: Temperature in degrees Celsius* of any drivers above which the fan is turned on.
+        validation: { bounds<>: [-20.0, 100.0] }
+
+      window_len:
+        type: int
+        default_value: 6
+        description: Moving average window length used to smooth out the temperature readings of each driver.
+        validation: { gt<>: 0 }
+
+  fan_turn_off_timeout:
+    type: double
+    default_value: 60.0
+    description: Minimal time in seconds, after which the fan may be turned off.
+    validation: { gt<>: 0.0 }
+
+  plugin_libs:
+    type: string_array
+    default_value: []
+    description: List with names of plugins that are used in the BT project.
+
+  ros_communication_timeout:
+    availability:
+      type: double
+      default_value: 1.0
+      description: Timeout seconds to wait for a service/action while initializing BT node.
+      validation: { gt<>: 0.0 }
+
+    response:
+      type: double
+      default_value: 1.0
+      description: Timeout seconds to receive a service/action response after call.
+      validation: { gt<>: 0.0 }
+
+  ros_plugin_libs:
+    type: string_array
+    default_value: []
+    description: List with names of ROS plugins that are used in a BT project.
+
+  shutdown_hosts_path:
+    type: string
+    default_value: ""
+    description: Path to a YAML file containing a list of hosts to request shutdown. To correctly format the YAML file, consult the README.
+
+  timer_frequency:
+    type: double
+    default_value: 10.0
+    description: Frequency in Hz at which safety tree will be ticked.
diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp
index 6fd54433..655e19e7 100644
--- a/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp
@@ -26,6 +26,8 @@
 
 #include "panther_msgs/msg/led_animation.hpp"
 
+#include "lights_manager_parameters.hpp"
+
 #include "husarion_ugv_utils/moving_average.hpp"
 
 #include <husarion_ugv_manager/behavior_tree_manager.hpp>
@@ -73,6 +75,9 @@ class LightsManagerNode : public rclcpp::Node
 
   float update_charging_anim_step_;
 
+  std::shared_ptr<lights_manager::ParamListener> param_listener_;
+  lights_manager::Params params_;
+
   rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_sub_;
   rclcpp::Subscription<BoolMsg>::SharedPtr e_stop_sub_;
   rclcpp::TimerBase::SharedPtr lights_tree_timer_;
diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp
index 51dcb3e2..3d2b247b 100644
--- a/husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp
@@ -29,6 +29,8 @@
 #include "panther_msgs/msg/robot_driver_state.hpp"
 #include "panther_msgs/msg/system_status.hpp"
 
+#include "safety_manager_parameters.hpp"
+
 #include "husarion_ugv_utils/moving_average.hpp"
 
 #include <husarion_ugv_manager/behavior_tree_manager.hpp>
@@ -89,6 +91,9 @@ class SafetyManagerNode : public rclcpp::Node
   int driver_temp_window_len_;
   float update_charging_anim_step_;
 
+  std::shared_ptr<safety_manager::ParamListener> param_listener_;
+  safety_manager::Params params_;
+
   rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_sub_;
   rclcpp::Subscription<RobotDriverStateMsg>::SharedPtr driver_state_sub_;
   rclcpp::Subscription<BoolMsg>::SharedPtr e_stop_sub_;
diff --git a/husarion_ugv_manager/package.xml b/husarion_ugv_manager/package.xml
index 3b5688d0..f6b39f90 100644
--- a/husarion_ugv_manager/package.xml
+++ b/husarion_ugv_manager/package.xml
@@ -19,6 +19,7 @@
 
   <depend>behaviortree_cpp</depend>
   <depend>behaviortree_ros2</depend>
+  <depend>generate_parameter_library</depend>
   <depend>husarion_ugv_utils</depend>
   <depend>iputils-ping</depend>
   <depend>libboost-dev</depend>
diff --git a/husarion_ugv_manager/src/lights_manager_node.cpp b/husarion_ugv_manager/src/lights_manager_node.cpp
index e4dbe992..730cf9d8 100644
--- a/husarion_ugv_manager/src/lights_manager_node.cpp
+++ b/husarion_ugv_manager/src/lights_manager_node.cpp
@@ -40,10 +40,11 @@ LightsManagerNode::LightsManagerNode(
 {
   RCLCPP_INFO(this->get_logger(), "Constructing node.");
 
-  DeclareParameters();
+  this->param_listener_ =
+    std::make_shared<lights_manager::ParamListener>(this->get_node_parameters_interface());
+  this->params_ = this->param_listener_->get_params();
 
-  const auto battery_percent_window_len =
-    this->get_parameter("battery.percent.window_len").as_int();
+  const auto battery_percent_window_len = this->params_.battery.percent.window_len;
 
   battery_percent_ma_ = std::make_unique<husarion_ugv_utils::MovingAverage<double>>(
     battery_percent_window_len, 1.0);
@@ -69,7 +70,7 @@ void LightsManagerNode::Initialize()
     "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&LightsManagerNode::EStopCB, this, _1));
 
-  const float timer_freq = this->get_parameter("timer_frequency").as_double();
+  const float timer_freq = this->params_.timer_frequency;
   const auto timer_period_ms =
     std::chrono::milliseconds(static_cast<unsigned>(1.0f / timer_freq * 1000));
 
@@ -79,40 +80,15 @@ void LightsManagerNode::Initialize()
   RCLCPP_INFO(this->get_logger(), "Initialized successfully.");
 }
 
-void LightsManagerNode::DeclareParameters()
-{
-  const auto husarion_ugv_manager_pkg_path =
-    ament_index_cpp::get_package_share_directory("husarion_ugv_manager");
-  const std::string default_bt_project_path = husarion_ugv_manager_pkg_path +
-                                              "/behavior_trees/LightsBT.btproj";
-  const std::vector<std::string> default_plugin_libs = {};
-
-  this->declare_parameter<std::string>("bt_project_path", default_bt_project_path);
-  this->declare_parameter<std::vector<std::string>>("plugin_libs", default_plugin_libs);
-  this->declare_parameter<std::vector<std::string>>("ros_plugin_libs", default_plugin_libs);
-  this->declare_parameter<double>("ros_communication_timeout.availability", 1.0);
-  this->declare_parameter<double>("ros_communication_timeout.response", 1.0);
-
-  this->declare_parameter<int>("battery.percent.window_len", 6);
-  this->declare_parameter<float>("battery.percent.threshold.low", 0.4);
-  this->declare_parameter<float>("battery.percent.threshold.critical", 0.1);
-  this->declare_parameter<float>("battery.animation_period.low", 30.0);
-  this->declare_parameter<float>("battery.animation_period.critical", 15.0);
-  this->declare_parameter<float>("battery.charging_anim_step", 0.1);
-  this->declare_parameter<float>("timer_frequency", 10.0);
-}
-
 void LightsManagerNode::RegisterBehaviorTree()
 {
-  const auto bt_project_path = this->get_parameter("bt_project_path").as_string();
+  const auto bt_project_path = this->params_.bt_project_path;
 
-  const auto plugin_libs = this->get_parameter("plugin_libs").as_string_array();
-  const auto ros_plugin_libs = this->get_parameter("ros_plugin_libs").as_string_array();
+  const auto plugin_libs = this->params_.plugin_libs;
+  const auto ros_plugin_libs = this->params_.ros_plugin_libs;
 
-  const auto service_availability_timeout =
-    this->get_parameter("ros_communication_timeout.availability").as_double();
-  const auto service_response_timeout =
-    this->get_parameter("ros_communication_timeout.response").as_double();
+  const auto service_availability_timeout = this->params_.ros_communication_timeout.availability;
+  const auto service_response_timeout = this->params_.ros_communication_timeout.response;
 
   BT::RosNodeParams params;
   params.nh = this->shared_from_this();
@@ -130,15 +106,11 @@ void LightsManagerNode::RegisterBehaviorTree()
 
 std::map<std::string, std::any> LightsManagerNode::CreateLightsInitialBlackboard()
 {
-  update_charging_anim_step_ = this->get_parameter("battery.charging_anim_step").as_double();
-  const float critical_battery_anim_period =
-    this->get_parameter("battery.animation_period.critical").as_double();
-  const float critical_battery_threshold_percent =
-    this->get_parameter("battery.percent.threshold.critical").as_double();
-  const float low_battery_anim_period =
-    this->get_parameter("battery.animation_period.low").as_double();
-  const float low_battery_threshold_percent =
-    this->get_parameter("battery.percent.threshold.low").as_double();
+  update_charging_anim_step_ = this->params_.battery.charging_anim_step;
+  const float critical_battery_anim_period = this->params_.battery.anim_period.critical;
+  const float critical_battery_threshold_percent = this->params_.battery.percent.threshold.critical;
+  const float low_battery_anim_period = this->params_.battery.anim_period.low;
+  const float low_battery_threshold_percent = this->params_.battery.percent.threshold.low;
 
   const std::string undefined_charging_anim_percent = "";
   const int undefined_anim_id = -1;
diff --git a/husarion_ugv_manager/src/safety_manager_node.cpp b/husarion_ugv_manager/src/safety_manager_node.cpp
index 0eaf05b5..56c69bfe 100644
--- a/husarion_ugv_manager/src/safety_manager_node.cpp
+++ b/husarion_ugv_manager/src/safety_manager_node.cpp
@@ -42,10 +42,12 @@ SafetyManagerNode::SafetyManagerNode(
 {
   RCLCPP_INFO(this->get_logger(), "Constructing node.");
 
-  DeclareParameters();
+  this->param_listener_ =
+    std::make_shared<safety_manager::ParamListener>(this->get_node_parameters_interface());
+  this->params_ = this->param_listener_->get_params();
 
-  const auto battery_temp_window_len = this->get_parameter("battery.temp.window_len").as_int();
-  const auto cpu_temp_window_len = this->get_parameter("cpu.temp.window_len").as_int();
+  const auto battery_temp_window_len = this->params_.battery.temp.window_len;
+  const auto cpu_temp_window_len = this->params_.cpu.temp.window_len;
 
   battery_temp_ma_ =
     std::make_unique<husarion_ugv_utils::MovingAverage<double>>(battery_temp_window_len);
@@ -55,7 +57,7 @@ SafetyManagerNode::SafetyManagerNode(
   safety_tree_manager_ = std::make_unique<BehaviorTreeManager>(
     "Safety", safety_initial_blackboard, 6666);
 
-  const auto shutdown_hosts_path = this->get_parameter("shutdown_hosts_path").as_string();
+  const auto shutdown_hosts_path = this->params_.shutdown_hosts_path;
   const std::map<std::string, std::any> shutdown_initial_blackboard = {
     {"SHUTDOWN_HOSTS_FILE", shutdown_hosts_path},
   };
@@ -96,7 +98,7 @@ void SafetyManagerNode::Initialize()
   //   Timers
   // -------------------------------
 
-  const float timer_freq = this->get_parameter("timer_frequency").as_double();
+  const float timer_freq = this->params_.timer_frequency;
   const auto timer_period_ms =
     std::chrono::milliseconds(static_cast<unsigned>(1.0f / timer_freq * 1000));
 
@@ -106,43 +108,15 @@ void SafetyManagerNode::Initialize()
   RCLCPP_INFO(this->get_logger(), "Initialized successfully.");
 }
 
-void SafetyManagerNode::DeclareParameters()
-{
-  const auto husarion_ugv_manager_pkg_path =
-    ament_index_cpp::get_package_share_directory("husarion_ugv_manager");
-  const std::string default_bt_project_path = husarion_ugv_manager_pkg_path +
-                                              "/behavior_trees/SafetyBT.btproj";
-  const std::vector<std::string> default_plugin_libs = {};
-
-  this->declare_parameter<std::string>("bt_project_path", default_bt_project_path);
-  this->declare_parameter<std::string>("shutdown_hosts_path", "");
-  this->declare_parameter<std::vector<std::string>>("plugin_libs", default_plugin_libs);
-  this->declare_parameter<std::vector<std::string>>("ros_plugin_libs", default_plugin_libs);
-  this->declare_parameter<double>("ros_communication_timeout.availability", 1.0);
-  this->declare_parameter<double>("ros_communication_timeout.response", 1.0);
-
-  this->declare_parameter<int>("battery.temp.window_len", 6);
-  this->declare_parameter<int>("cpu.temp.window_len", 6);
-  this->declare_parameter<float>("cpu.temp.fan_on", 70.0);
-  this->declare_parameter<float>("cpu.temp.fan_off", 60.0);
-  this->declare_parameter<int>("driver.temp.window_len", 6);
-  this->declare_parameter<float>("driver.temp.fan_on", 45.0);
-  this->declare_parameter<float>("driver.temp.fan_off", 35.0);
-  this->declare_parameter<float>("timer_frequency", 10.0);
-  this->declare_parameter<float>("fan_turn_off_timeout", 60.0);
-}
-
 void SafetyManagerNode::RegisterBehaviorTree()
 {
-  const auto bt_project_path = this->get_parameter("bt_project_path").as_string();
+  const auto bt_project_path = this->params_.bt_project_path;
 
-  const auto plugin_libs = this->get_parameter("plugin_libs").as_string_array();
-  const auto ros_plugin_libs = this->get_parameter("ros_plugin_libs").as_string_array();
+  const auto plugin_libs = this->params_.plugin_libs;
+  const auto ros_plugin_libs = this->params_.ros_plugin_libs;
 
-  const auto service_availability_timeout =
-    this->get_parameter("ros_communication_timeout.availability").as_double();
-  const auto service_response_timeout =
-    this->get_parameter("ros_communication_timeout.response").as_double();
+  const auto service_availability_timeout = this->params_.ros_communication_timeout.availability;
+  const auto service_response_timeout = this->params_.ros_communication_timeout.response;
 
   BT::RosNodeParams params;
   params.nh = this->shared_from_this();
@@ -160,11 +134,11 @@ void SafetyManagerNode::RegisterBehaviorTree()
 
 std::map<std::string, std::any> SafetyManagerNode::CreateSafetyInitialBlackboard()
 {
-  const float cpu_fan_on_temp = this->get_parameter("cpu.temp.fan_on").as_double();
-  const float cpu_fan_off_temp = this->get_parameter("cpu.temp.fan_off").as_double();
-  const float driver_fan_on_temp = this->get_parameter("driver.temp.fan_on").as_double();
-  const float driver_fan_off_temp = this->get_parameter("driver.temp.fan_off").as_double();
-  const float fan_turn_off_timeout = this->get_parameter("fan_turn_off_timeout").as_double();
+  const float cpu_fan_on_temp = this->params_.cpu.temp.fan_on;
+  const float cpu_fan_off_temp = this->params_.cpu.temp.fan_off;
+  const float driver_fan_on_temp = this->params_.driver.temp.fan_on;
+  const float driver_fan_off_temp = this->params_.driver.temp.fan_off;
+  const float fan_turn_off_timeout = this->params_.fan_turn_off_timeout;
 
   const std::map<std::string, std::any> safety_initial_bb = {
     {"CPU_FAN_OFF_TEMP", cpu_fan_off_temp},
@@ -221,7 +195,7 @@ void SafetyManagerNode::RobotDriverStateCB(const RobotDriverStateMsg::SharedPtr
     if (driver_temp_ma_.find(driver.name) == driver_temp_ma_.end()) {
       RCLCPP_DEBUG(
         this->get_logger(), "Creating moving average for driver '%s'", driver.name.c_str());
-      const auto driver_temp_window_len = this->get_parameter("driver.temp.window_len").as_int();
+      const auto driver_temp_window_len = this->params_.driver.temp.window_len;
       driver_temp_ma_[driver.name] =
         std::make_unique<husarion_ugv_utils::MovingAverage<double>>(driver_temp_window_len);
     }

From 66de8ed5338620670c40becaabe0e552c62eeba1 Mon Sep 17 00:00:00 2001
From: Stefan <139364969+BOOTCFG@users.noreply.github.com>
Date: Fri, 8 Nov 2024 16:06:16 +0100
Subject: [PATCH 083/100] Overwriting robot description  (#440)

* create overwrite_robot_description.launch

* fix namespace
---
 .../overwrite_robot_description.launch.py     | 180 ++++++++++++++++++
 1 file changed, 180 insertions(+)
 create mode 100644 panther_description/launch/overwrite_robot_description.launch.py

diff --git a/panther_description/launch/overwrite_robot_description.launch.py b/panther_description/launch/overwrite_robot_description.launch.py
new file mode 100644
index 00000000..a7f5cff9
--- /dev/null
+++ b/panther_description/launch/overwrite_robot_description.launch.py
@@ -0,0 +1,180 @@
+#!/usr/bin/env python3
+
+# Copyright 2020 ros2_control Development Team
+# 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.
+
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.substitutions import (
+    Command,
+    EnvironmentVariable,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression,
+)
+from launch_ros.actions import SetParameter
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+
+    battery_config_path = LaunchConfiguration("battery_config_path")
+    declare_battery_config_path_arg = DeclareLaunchArgument(
+        "battery_config_path",
+        description=(
+            "Path to the Ignition LinearBatteryPlugin configuration file. "
+            "This configuration is intended for use in simulations only."
+        ),
+        default_value="",
+    )
+
+    components_config_path = LaunchConfiguration("components_config_path")
+    declare_components_config_path_arg = DeclareLaunchArgument(
+        "components_config_path",
+        default_value=PathJoinSubstitution(
+            [FindPackageShare("panther_description"), "config", "components.yaml"]
+        ),
+        description=(
+            "Additional components configuration file. Components described in this file "
+            "are dynamically included in Panther's urdf."
+            "Panther options are described here "
+            "https://husarion.com/manuals/panther/panther-options/"
+        ),
+    )
+
+    wheel_type = LaunchConfiguration("wheel_type")
+    controller_config_path = LaunchConfiguration("controller_config_path")
+    declare_controller_config_path_arg = DeclareLaunchArgument(
+        "controller_config_path",
+        default_value=PathJoinSubstitution(
+            [
+                FindPackageShare("husarion_ugv_controller"),
+                "config",
+                PythonExpression(["'", wheel_type, "_controller.yaml'"]),
+            ]
+        ),
+        description=(
+            "Path to controller configuration file. By default, it is located in"
+            " 'husarion_ugv_controller/config/{wheel_type}_controller.yaml'. You can also specify"
+            " the path to your custom controller configuration file here. "
+        ),
+    )
+
+    namespace = LaunchConfiguration("namespace")
+    declare_namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Add namespace to all launched nodes.",
+    )
+
+    use_sim = LaunchConfiguration("use_sim")
+    declare_use_sim_arg = DeclareLaunchArgument(
+        "use_sim",
+        default_value="False",
+        description="Whether simulation is used.",
+        choices=["True", "true", "False", "false"],
+    )
+
+    wheel_config_path = LaunchConfiguration("wheel_config_path")
+    declare_wheel_config_path_arg = DeclareLaunchArgument(
+        "wheel_config_path",
+        default_value=PathJoinSubstitution(
+            [
+                FindPackageShare("panther_description"),
+                "config",
+                PythonExpression(["'", wheel_type, ".yaml'"]),
+            ]
+        ),
+        description=(
+            "Path to wheel configuration file. By default, it is located in "
+            "'panther_description/config/{wheel_type}.yaml'. You can also specify the path "
+            "to your custom wheel configuration file here. "
+        ),
+    )
+
+    declare_wheel_type_arg = DeclareLaunchArgument(
+        "wheel_type",
+        default_value="WH01",
+        description=(
+            "Specify the wheel type. If the selected wheel type is not 'custom', "
+            "the 'wheel_config_path' and 'controller_config_path' arguments will be "
+            "automatically adjusted and can be omitted."
+        ),
+        choices=["WH01", "WH02", "WH04", "custom"],
+    )
+
+    # Get URDF via xacro
+    imu_pos_x = os.environ.get("ROBOT_IMU_LOCALIZATION_X", "0.168")
+    imu_pos_y = os.environ.get("ROBOT_IMU_LOCALIZATION_Y", "0.028")
+    imu_pos_z = os.environ.get("ROBOT_IMU_LOCALIZATION_Z", "0.083")
+    imu_rot_r = os.environ.get("ROBOT_IMU_ORIENTATION_R", "3.14")
+    imu_rot_p = os.environ.get("ROBOT_IMU_ORIENTATION_P", "-1.57")
+    imu_rot_y = os.environ.get("ROBOT_IMU_ORIENTATION_Y", "0.0")
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("panther_description"), "urdf", "panther.urdf.xacro"]
+            ),
+            " use_sim:=",
+            use_sim,
+            " wheel_config_file:=",
+            wheel_config_path,
+            " controller_config_file:=",
+            controller_config_path,
+            " battery_config_file:=",
+            battery_config_path,
+            " imu_xyz:=",
+            f"'{imu_pos_x} {imu_pos_y} {imu_pos_z}'",
+            " imu_rpy:=",
+            f"'{imu_rot_r} {imu_rot_p} {imu_rot_y}'",
+            " namespace:=",
+            namespace,
+            " components_config_path:=",
+            components_config_path,
+        ]
+    )
+
+    namespace_ext = PythonExpression(["'", namespace, "' + '/' if '", namespace, "' else ''"])
+
+    set_robot_description = ExecuteProcess(
+        cmd=[
+            "ros2",
+            "param",
+            "set",
+            [namespace_ext, "robot_state_publisher"],
+            "robot_description",
+            robot_description_content,
+        ],
+        output="screen",
+    )
+
+    actions = [
+        declare_battery_config_path_arg,
+        declare_components_config_path_arg,
+        declare_wheel_type_arg,  # wheel_type is used by controller_config_path
+        declare_controller_config_path_arg,
+        declare_namespace_arg,
+        declare_use_sim_arg,
+        declare_wheel_config_path_arg,
+        SetParameter(name="use_sim_time", value=use_sim),
+        set_robot_description,
+    ]
+
+    return LaunchDescription(actions)

From b0600b35af76df823c896661ea15142fcacc6d6e Mon Sep 17 00:00:00 2001
From: Milosz Lagan <milosz.lagan@husarion.com>
Date: Tue, 19 Nov 2024 12:08:15 +0000
Subject: [PATCH 084/100] Parameter type casts and typo fixes

---
 .../config/lights_controller_parameters.yaml    |  2 +-
 .../src/lights_controller_node.cpp              |  2 +-
 .../config/lights_manager_parameters.yaml       | 17 ++++++++---------
 .../config/safety_manager_parameters.yaml       |  9 ++++-----
 .../src/lights_manager_node.cpp                 |  9 +++++----
 .../src/safety_manager_node.cpp                 | 12 ++++++------
 6 files changed, 25 insertions(+), 26 deletions(-)

diff --git a/husarion_ugv_lights/config/lights_controller_parameters.yaml b/husarion_ugv_lights/config/lights_controller_parameters.yaml
index c4b90ab0..0994c7b4 100644
--- a/husarion_ugv_lights/config/lights_controller_parameters.yaml
+++ b/husarion_ugv_lights/config/lights_controller_parameters.yaml
@@ -2,7 +2,7 @@ lights_controller:
   animations_config_path:
     type: string
     default_value: ""
-    description: Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations.
+    description: Path to a YAML file with a description of LED configuration. This file includes definition of robot panels, virtual segments and default animations.
     validation: { not_empty<> }
 
   controller_frequency:
diff --git a/husarion_ugv_lights/src/lights_controller_node.cpp b/husarion_ugv_lights/src/lights_controller_node.cpp
index e1d51224..9306ff0c 100644
--- a/husarion_ugv_lights/src/lights_controller_node.cpp
+++ b/husarion_ugv_lights/src/lights_controller_node.cpp
@@ -56,7 +56,7 @@ LightsControllerNode::LightsControllerNode(const rclcpp::NodeOptions & options)
 
   const auto animations_config_path = this->params_.animations_config_path;
   const auto user_led_animations_path = this->params_.user_led_animations_path;
-  const float controller_freq = this->params_.controller_frequency;
+  const float controller_freq = static_cast<float>(this->params_.controller_frequency);
 
   YAML::Node led_config_desc = YAML::LoadFile(animations_config_path);
 
diff --git a/husarion_ugv_manager/config/lights_manager_parameters.yaml b/husarion_ugv_manager/config/lights_manager_parameters.yaml
index e60a404c..efe28cfd 100644
--- a/husarion_ugv_manager/config/lights_manager_parameters.yaml
+++ b/husarion_ugv_manager/config/lights_manager_parameters.yaml
@@ -10,51 +10,50 @@ lights_manager:
       critical:
         type: double
         default_value: 15.0
-        description: Time in seconds to wait before repeating animation, indicating a critical Battery state.
+        description: Time in seconds to wait before repeating animation, indicating a critical battery state.
         validation: { gt<>: 0.0 }
 
       low:
         type: double
         default_value: 30.0
-        description: Time in seconds to wait before repeating the animation, indicating a low Battery state.
+        description: Time in seconds to wait before repeating the animation, indicating a low battery state.
         validation: { gt<>: 0.0 }
 
     percent:
       window_len:
         type: int
         default_value: 6
-        description: Moving average window length used to smooth out Battery percentage readings.
+        description: Moving average window length used to smooth out battery percentage readings.
         validation: { gt<>: 0 }
 
       threshold:
         critical:
           type: double
           default_value: 0.1
-          description: If the Battery percentage drops below this value, an animation indicating a critical Battery state will start being displayed.
+          description: If the battery percentage drops below this value, an animation indicating a critical battery state will start being displayed.
           validation: { bounds<>: [0.0, 1.0] }
 
         low:
           type: double
           default_value: 0.4
-          description: If the Battery percentage drops below this value, the animation indicating a low Battery state will start being displayed.
+          description: If the battery percentage drops below this value, the animation indicating a low battery state will start being displayed.
           validation: { bounds<>: [0.0, 1.0] }
 
   bt_project_path:
     type: string
     default_value: ""
     description: Path to a BehaviorTree project.
-    validation: { not_empty<> }
 
   plugin_libs:
     type: string_array
     default_value: []
-    description: List with names of plugins that are used in the BT project.
+    description: A list with names of plugins that are used in the BehaviorTree project.
 
   ros_communication_timeout:
     availability:
       type: double
       default_value: 1.0
-      description: Timeout in seconds to wait for a service/action while initializing BT node.
+      description: Timeout in seconds to wait for a service/action while initializing a BehaviorTree node.
       validation: { gt<>: 0.0 }
 
     response:
@@ -66,7 +65,7 @@ lights_manager:
   ros_plugin_libs:
     type: string_array
     default_value: []
-    description: List with names of ROS plugins that are used in a BT project.
+    description: A list with names of ROS plugins that are used in a BehaviorTree project.
 
   timer_frequency:
     type: double
diff --git a/husarion_ugv_manager/config/safety_manager_parameters.yaml b/husarion_ugv_manager/config/safety_manager_parameters.yaml
index 4010ee12..068a4d9d 100644
--- a/husarion_ugv_manager/config/safety_manager_parameters.yaml
+++ b/husarion_ugv_manager/config/safety_manager_parameters.yaml
@@ -11,7 +11,6 @@ safety_manager:
     type: string
     default_value: ""
     description: Path to a BehaviorTree project.
-    validation: { not_empty<> }
 
   cpu:
     temp:
@@ -44,7 +43,7 @@ safety_manager:
       fan_on:
         type: double
         default_value: 45.0
-        description: Temperature in degrees Celsius* of any drivers above which the fan is turned on.
+        description: Temperature in degrees Celsius of any drivers above which the fan is turned on.
         validation: { bounds<>: [-20.0, 100.0] }
 
       window_len:
@@ -62,13 +61,13 @@ safety_manager:
   plugin_libs:
     type: string_array
     default_value: []
-    description: List with names of plugins that are used in the BT project.
+    description: A list with names of plugins that are used in the a BehaviorTree project.
 
   ros_communication_timeout:
     availability:
       type: double
       default_value: 1.0
-      description: Timeout seconds to wait for a service/action while initializing BT node.
+      description: Timeout seconds to wait for a service/action while initializing a BehaviorTree node.
       validation: { gt<>: 0.0 }
 
     response:
@@ -80,7 +79,7 @@ safety_manager:
   ros_plugin_libs:
     type: string_array
     default_value: []
-    description: List with names of ROS plugins that are used in a BT project.
+    description: A list with names of ROS plugins that are used in a BehaviorTree project.
 
   shutdown_hosts_path:
     type: string
diff --git a/husarion_ugv_manager/src/lights_manager_node.cpp b/husarion_ugv_manager/src/lights_manager_node.cpp
index 730cf9d8..4cdd5484 100644
--- a/husarion_ugv_manager/src/lights_manager_node.cpp
+++ b/husarion_ugv_manager/src/lights_manager_node.cpp
@@ -107,10 +107,11 @@ void LightsManagerNode::RegisterBehaviorTree()
 std::map<std::string, std::any> LightsManagerNode::CreateLightsInitialBlackboard()
 {
   update_charging_anim_step_ = this->params_.battery.charging_anim_step;
-  const float critical_battery_anim_period = this->params_.battery.anim_period.critical;
-  const float critical_battery_threshold_percent = this->params_.battery.percent.threshold.critical;
-  const float low_battery_anim_period = this->params_.battery.anim_period.low;
-  const float low_battery_threshold_percent = this->params_.battery.percent.threshold.low;
+  const double critical_battery_anim_period = this->params_.battery.anim_period.critical;
+  const double critical_battery_threshold_percent =
+    this->params_.battery.percent.threshold.critical;
+  const double low_battery_anim_period = this->params_.battery.anim_period.low;
+  const double low_battery_threshold_percent = this->params_.battery.percent.threshold.low;
 
   const std::string undefined_charging_anim_percent = "";
   const int undefined_anim_id = -1;
diff --git a/husarion_ugv_manager/src/safety_manager_node.cpp b/husarion_ugv_manager/src/safety_manager_node.cpp
index 56c69bfe..63965f4f 100644
--- a/husarion_ugv_manager/src/safety_manager_node.cpp
+++ b/husarion_ugv_manager/src/safety_manager_node.cpp
@@ -98,7 +98,7 @@ void SafetyManagerNode::Initialize()
   //   Timers
   // -------------------------------
 
-  const float timer_freq = this->params_.timer_frequency;
+  const double timer_freq = this->params_.timer_frequency;
   const auto timer_period_ms =
     std::chrono::milliseconds(static_cast<unsigned>(1.0f / timer_freq * 1000));
 
@@ -134,11 +134,11 @@ void SafetyManagerNode::RegisterBehaviorTree()
 
 std::map<std::string, std::any> SafetyManagerNode::CreateSafetyInitialBlackboard()
 {
-  const float cpu_fan_on_temp = this->params_.cpu.temp.fan_on;
-  const float cpu_fan_off_temp = this->params_.cpu.temp.fan_off;
-  const float driver_fan_on_temp = this->params_.driver.temp.fan_on;
-  const float driver_fan_off_temp = this->params_.driver.temp.fan_off;
-  const float fan_turn_off_timeout = this->params_.fan_turn_off_timeout;
+  const double cpu_fan_on_temp = this->params_.cpu.temp.fan_on;
+  const double cpu_fan_off_temp = this->params_.cpu.temp.fan_off;
+  const double driver_fan_on_temp = this->params_.driver.temp.fan_on;
+  const double driver_fan_off_temp = this->params_.driver.temp.fan_off;
+  const double fan_turn_off_timeout = this->params_.fan_turn_off_timeout;
 
   const std::map<std::string, std::any> safety_initial_bb = {
     {"CPU_FAN_OFF_TEMP", cpu_fan_off_temp},

From 8faab9ce58560ef7179b484f402d5dbce460b1c3 Mon Sep 17 00:00:00 2001
From: Milosz Lagan <milosz.lagan@husarion.com>
Date: Tue, 19 Nov 2024 17:15:18 +0000
Subject: [PATCH 085/100] Rework moving average logic

---
 .../husarion_ugv_utils/moving_average.hpp     | 20 ++++++++--------
 .../test/test_moving_average.cpp              | 23 +++++++++++++++++--
 2 files changed, 31 insertions(+), 12 deletions(-)

diff --git a/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
index 0a6c7636..03fe6b07 100644
--- a/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
+++ b/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
@@ -25,40 +25,40 @@ class MovingAverage
 {
 public:
   MovingAverage(const std::size_t window_size = 5, const T initial_value = T(0))
-  : window_size_(window_size), initial_value_(initial_value), sum_(T(0))
+  : window_size_(window_size), initial_value_(initial_value)
   {
   }
 
   void Roll(const T value)
   {
     values_.push_back(value);
-    sum_ += value;
 
     if (values_.size() > window_size_) {
-      sum_ -= values_.front();
       values_.pop_front();
     }
   }
 
-  void Reset()
-  {
-    values_.erase(values_.begin(), values_.end());
-    sum_ = T(0);
-  }
+  void Reset() { values_.erase(values_.begin(), values_.end()); }
 
   T GetAverage() const
   {
     if (values_.size() == 0) {
       return initial_value_;
     }
-    return sum_ / static_cast<T>(values_.size());
+
+    T sum = T(0);
+
+    for (const auto & value : values_) {
+      sum += value / static_cast<T>(values_.size());
+    }
+
+    return sum;
   }
 
 private:
   const std::size_t window_size_;
   std::deque<T> values_;
   const T initial_value_;
-  T sum_;
 };
 
 }  // namespace husarion_ugv_utils
diff --git a/husarion_ugv_utils/test/test_moving_average.cpp b/husarion_ugv_utils/test/test_moving_average.cpp
index 653483ce..e520bc80 100644
--- a/husarion_ugv_utils/test/test_moving_average.cpp
+++ b/husarion_ugv_utils/test/test_moving_average.cpp
@@ -61,12 +61,12 @@ TEST(TestMovingAverage, TestHighOverload)
 
   double sum = 0.0;
   for (std::size_t i = 1; i <= window_len * 10; i++) {
-    sum += double(i);
+    sum += double(i) / double(window_len);
     ma.Roll(double(i));
 
     // test every 1000 rolls expected average
     if (i % window_len == 0) {
-      EXPECT_EQ(sum / double(window_len), ma.GetAverage());
+      EXPECT_LT(sum - ma.GetAverage(), std::numeric_limits<double>::epsilon());
       sum = 0.0;
     }
   }
@@ -107,6 +107,25 @@ TEST(TestMovingAverage, TestResetToInitialValue)
   EXPECT_EQ(7.0, ma.GetAverage());
 }
 
+TEST(TestMovingAverage, TestInfInjectionHandling)
+{
+  husarion_ugv_utils::MovingAverage<double> ma(4);
+  ma.Roll(1.0);
+  ma.Roll(2.0);
+  ma.Roll(3.0);
+  ma.Roll(4.0);
+  EXPECT_EQ(2.5, ma.GetAverage());
+
+  ma.Roll(std::numeric_limits<double>::infinity());
+  EXPECT_EQ(std::numeric_limits<double>::infinity(), ma.GetAverage());
+
+  ma.Roll(1.0);
+  ma.Roll(2.0);
+  ma.Roll(3.0);
+  ma.Roll(4.0);
+  EXPECT_EQ(2.5, ma.GetAverage());
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);

From 3c3a3accb2434851fb85ba0d573278a004d59077 Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Wed, 20 Nov 2024 11:59:40 +0000
Subject: [PATCH 086/100] use configs from /config dir

---
 .../launch/controller.launch.py               | 17 +++++++++-------
 .../config/user_animations.yaml               | 20 +++++++++++++++++++
 husarion_ugv_lights/launch/lights.launch.py   | 11 ++++++----
 .../launch/localization.launch.py             |  2 +-
 husarion_ugv_manager/launch/manager.launch.py | 13 ++++++------
 5 files changed, 45 insertions(+), 18 deletions(-)
 create mode 100644 husarion_ugv_lights/config/user_animations.yaml

diff --git a/husarion_ugv_controller/launch/controller.launch.py b/husarion_ugv_controller/launch/controller.launch.py
index 2e61a370..70d262ef 100644
--- a/husarion_ugv_controller/launch/controller.launch.py
+++ b/husarion_ugv_controller/launch/controller.launch.py
@@ -34,6 +34,11 @@
 
 
 def generate_launch_description():
+    husarion_ugv_desctiption_pkg = FindPackageShare("husarion_ugv_description")
+    husarion_ugv_controller_common_dir = PathJoinSubstitution(
+        ["/config", "husarion_ugv_controller"]
+    )
+
     battery_config_path = LaunchConfiguration("battery_config_path")
     declare_battery_config_path_arg = DeclareLaunchArgument(
         "battery_config_path",
@@ -48,7 +53,7 @@ def generate_launch_description():
     declare_components_config_path_arg = DeclareLaunchArgument(
         "components_config_path",
         default_value=PathJoinSubstitution(
-            [FindPackageShare("panther_description"), "config", "components.yaml"]
+            [husarion_ugv_desctiption_pkg, "config", "components.yaml"]
         ),
         description=(
             "Additional components configuration file. Components described in this file "
@@ -64,7 +69,7 @@ def generate_launch_description():
         "controller_config_path",
         default_value=PathJoinSubstitution(
             [
-                FindPackageShare("husarion_ugv_controller"),
+                husarion_ugv_controller_common_dir,
                 "config",
                 PythonExpression(["'", wheel_type, "_controller.yaml'"]),
             ]
@@ -112,20 +117,19 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
-    robot_description_pkg = PythonExpression(["'", robot_model, "_description'"])
     wheel_config_path = LaunchConfiguration("wheel_config_path")
     declare_wheel_config_path_arg = DeclareLaunchArgument(
         "wheel_config_path",
         default_value=PathJoinSubstitution(
             [
-                FindPackageShare(robot_description_pkg),
+                FindPackageShare(husarion_ugv_desctiption_pkg),
                 "config",
                 PythonExpression(["'", wheel_type, ".yaml'"]),
             ]
         ),
         description=(
             "Path to wheel configuration file. By default, it is located in "
-            "'panther_description/config/{wheel_type}.yaml'. You can also specify the path "
+            "'husarion_ugv_description/config/{wheel_type}.yaml'. You can also specify the path "
             "to your custom wheel configuration file here. "
         ),
     )
@@ -143,7 +147,6 @@ def generate_launch_description():
     )
 
     # Get URDF via xacro
-    robot_description_pkg = PythonExpression(["'", robot_model, "_description'"])
     robot_description_file = PythonExpression(["'", robot_model, ".urdf.xacro'"])
     imu_pos_x = os.environ.get("ROBOT_IMU_LOCALIZATION_X", "0.168")
     imu_pos_y = os.environ.get("ROBOT_IMU_LOCALIZATION_Y", "0.028")
@@ -157,7 +160,7 @@ def generate_launch_description():
             " ",
             PathJoinSubstitution(
                 [
-                    FindPackageShare(robot_description_pkg),
+                    FindPackageShare(husarion_ugv_desctiption_pkg),
                     "urdf",
                     robot_description_file,
                 ]
diff --git a/husarion_ugv_lights/config/user_animations.yaml b/husarion_ugv_lights/config/user_animations.yaml
new file mode 100644
index 00000000..01771347
--- /dev/null
+++ b/husarion_ugv_lights/config/user_animations.yaml
@@ -0,0 +1,20 @@
+# This file is a placeholder for user-defined animations for the Husarion UGV
+# robots lights system. Users can define their own light animations in this
+# YAML file.
+#
+# For more examples and detailed documentation, please visit:
+# https://github.com/husarion/panther_ros/blob/ros2/panther_lights/CONFIGURATION.md#defining-animations
+#
+# Example including a simple custom animation:
+#
+# user_animations:
+#   - id: 21
+#     name: CUSTOM_IMAGE_ANIMATION
+#     priority: 3
+#     animations:
+#       - type: panther_lights::ImageAnimation
+#         segments: all
+#         animation:
+#           image: /config/husarion_ugv_lights/animations/example.png
+#           duration: 3
+#           repeat: 1
diff --git a/husarion_ugv_lights/launch/lights.launch.py b/husarion_ugv_lights/launch/lights.launch.py
index fa45aa53..8fd1f466 100644
--- a/husarion_ugv_lights/launch/lights.launch.py
+++ b/husarion_ugv_lights/launch/lights.launch.py
@@ -33,13 +33,14 @@
 
 def generate_launch_description():
     robot_model = LaunchConfiguration("robot_model")
-    lights_pkg = FindPackageShare("husarion_ugv_lights")
+    husarion_ugv_lights_pkg = FindPackageShare("husarion_ugv_lights")
+    husarion_ugv_lights_common_dir = PathJoinSubstitution(["/config", "husarion_ugv_lights"])
     animations_config = PythonExpression(["'", robot_model, "_animations.yaml'"])
 
     animations_config_path = LaunchConfiguration("animations_config_path")
     declare_animations_config_path_arg = DeclareLaunchArgument(
         "animations_config_path",
-        default_value=PathJoinSubstitution([lights_pkg, "config", animations_config]),
+        default_value=PathJoinSubstitution([husarion_ugv_lights_pkg, "config", animations_config]),
         description="Path to a YAML file with a description of led configuration.",
     )
 
@@ -69,12 +70,14 @@ def generate_launch_description():
     user_led_animations_path = LaunchConfiguration("user_led_animations_path")
     declare_user_led_animations_path_arg = DeclareLaunchArgument(
         "user_led_animations_path",
-        default_value="",
+        default_value=PathJoinSubstitution(
+            [husarion_ugv_lights_common_dir, "config", "user_animations.yaml"]
+        ),
         description="Path to a YAML file with a description of the user defined animations.",
     )
 
     driver_config = PythonExpression(["'", robot_model, "_driver.yaml'"])
-    driver_config_path = PathJoinSubstitution([lights_pkg, "config", driver_config])
+    driver_config_path = PathJoinSubstitution([husarion_ugv_lights_pkg, "config", driver_config])
     lights_container = ComposableNodeContainer(
         package="rclcpp_components",
         name="lights_container",
diff --git a/husarion_ugv_localization/launch/localization.launch.py b/husarion_ugv_localization/launch/localization.launch.py
index e948eb28..f03843e7 100644
--- a/husarion_ugv_localization/launch/localization.launch.py
+++ b/husarion_ugv_localization/launch/localization.launch.py
@@ -90,7 +90,7 @@ def generate_launch_description():
     declare_localization_config_path_arg = DeclareLaunchArgument(
         "localization_config_path",
         default_value=PathJoinSubstitution(
-            [FindPackageShare("husarion_ugv_localization"), "config", localization_config_filename]
+            ["/config", "husarion_ugv_localization", "config", localization_config_filename]
         ),
         description="Specify the path to the localization configuration file.",
     )
diff --git a/husarion_ugv_manager/launch/manager.launch.py b/husarion_ugv_manager/launch/manager.launch.py
index 1412e924..60c0b4f5 100644
--- a/husarion_ugv_manager/launch/manager.launch.py
+++ b/husarion_ugv_manager/launch/manager.launch.py
@@ -28,13 +28,14 @@
 
 def generate_launch_description():
 
-    husarion_ugv_manager_dir = FindPackageShare("husarion_ugv_manager")
+    husarion_ugv_manager_pkg = FindPackageShare("husarion_ugv_manager")
+    husarion_ugv_manager_common_dir = PathJoinSubstitution(["/config", "husarion_ugv_manager"])
 
     lights_bt_project_path = LaunchConfiguration("lights_bt_project_path")
     declare_lights_bt_project_path_arg = DeclareLaunchArgument(
         "lights_bt_project_path",
         default_value=PathJoinSubstitution(
-            [husarion_ugv_manager_dir, "behavior_trees", "LightsBT.btproj"]
+            [husarion_ugv_manager_common_dir, "behavior_trees", "LightsBT.btproj"]
         ),
         description="Path to BehaviorTree project file, responsible for lights management.",
     )
@@ -50,7 +51,7 @@ def generate_launch_description():
     declare_safety_bt_project_path_arg = DeclareLaunchArgument(
         "safety_bt_project_path",
         default_value=PathJoinSubstitution(
-            [husarion_ugv_manager_dir, "behavior_trees", "SafetyBT.btproj"]
+            [husarion_ugv_manager_pkg, "behavior_trees", "SafetyBT.btproj"]
         ),
         description="Path to BehaviorTree project file, responsible for safety and shutdown management.",
     )
@@ -60,7 +61,7 @@ def generate_launch_description():
         "shutdown_hosts_config_path",
         default_value=PathJoinSubstitution(
             [
-                FindPackageShare("husarion_ugv_manager"),
+                husarion_ugv_manager_common_dir,
                 "config",
                 "shutdown_hosts.yaml",
             ]
@@ -80,7 +81,7 @@ def generate_launch_description():
         executable="lights_manager_node",
         name="lights_manager",
         parameters=[
-            PathJoinSubstitution([husarion_ugv_manager_dir, "config", "lights_manager.yaml"]),
+            PathJoinSubstitution([husarion_ugv_manager_pkg, "config", "lights_manager.yaml"]),
             {"bt_project_path": lights_bt_project_path},
         ],
         namespace=namespace,
@@ -92,7 +93,7 @@ def generate_launch_description():
         executable="safety_manager_node",
         name="safety_manager",
         parameters=[
-            PathJoinSubstitution([husarion_ugv_manager_dir, "config", "safety_manager.yaml"]),
+            PathJoinSubstitution([husarion_ugv_manager_pkg, "config", "safety_manager.yaml"]),
             {
                 "bt_project_path": safety_bt_project_path,
                 "shutdown_hosts_path": shutdown_hosts_config_path,

From 9d5be9fb71d001e75682b0c35e4ef413d49f54de Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Thu, 21 Nov 2024 07:53:09 +0000
Subject: [PATCH 087/100] component file

---
 .../launch/controller.launch.py               | 33 ++++++++++---------
 1 file changed, 18 insertions(+), 15 deletions(-)

diff --git a/husarion_ugv_controller/launch/controller.launch.py b/husarion_ugv_controller/launch/controller.launch.py
index 70d262ef..661f67cd 100644
--- a/husarion_ugv_controller/launch/controller.launch.py
+++ b/husarion_ugv_controller/launch/controller.launch.py
@@ -34,11 +34,24 @@
 
 
 def generate_launch_description():
-    husarion_ugv_desctiption_pkg = FindPackageShare("husarion_ugv_description")
     husarion_ugv_controller_common_dir = PathJoinSubstitution(
         ["/config", "husarion_ugv_controller"]
     )
 
+    robot_model = LaunchConfiguration("robot_model")
+    robot_model_dict = {"LNX": "lynx", "PTH": "panther"}
+    robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
+
+    robot_description_pkg = PythonExpression(["'", robot_model, "_description'"])
+    robot_description_common_dir = PathJoinSubstitution(["/config", robot_description_pkg])
+
+    declare_robot_model_arg = DeclareLaunchArgument(
+        "robot_model",
+        default_value=robot_model_dict[robot_model_env],
+        description="Specify robot model",
+        choices=["lynx", "panther"],
+    )
+
     battery_config_path = LaunchConfiguration("battery_config_path")
     declare_battery_config_path_arg = DeclareLaunchArgument(
         "battery_config_path",
@@ -53,7 +66,7 @@ def generate_launch_description():
     declare_components_config_path_arg = DeclareLaunchArgument(
         "components_config_path",
         default_value=PathJoinSubstitution(
-            [husarion_ugv_desctiption_pkg, "config", "components.yaml"]
+            [robot_description_common_dir, "config", "components.yaml"]
         ),
         description=(
             "Additional components configuration file. Components described in this file "
@@ -99,16 +112,6 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
-    robot_model = LaunchConfiguration("robot_model")
-    robot_model_dict = {"LNX": "lynx", "PTH": "panther"}
-    robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
-    declare_robot_model_arg = DeclareLaunchArgument(
-        "robot_model",
-        default_value=robot_model_dict[robot_model_env],
-        description="Specify robot model",
-        choices=["lynx", "panther"],
-    )
-
     use_sim = LaunchConfiguration("use_sim")
     declare_use_sim_arg = DeclareLaunchArgument(
         "use_sim",
@@ -122,14 +125,14 @@ def generate_launch_description():
         "wheel_config_path",
         default_value=PathJoinSubstitution(
             [
-                FindPackageShare(husarion_ugv_desctiption_pkg),
+                FindPackageShare(robot_description_pkg),
                 "config",
                 PythonExpression(["'", wheel_type, ".yaml'"]),
             ]
         ),
         description=(
             "Path to wheel configuration file. By default, it is located in "
-            "'husarion_ugv_description/config/{wheel_type}.yaml'. You can also specify the path "
+            "'panther_description/config/{wheel_type}.yaml'. You can also specify the path "
             "to your custom wheel configuration file here. "
         ),
     )
@@ -160,7 +163,7 @@ def generate_launch_description():
             " ",
             PathJoinSubstitution(
                 [
-                    FindPackageShare(husarion_ugv_desctiption_pkg),
+                    FindPackageShare(robot_description_pkg),
                     "urdf",
                     robot_description_file,
                 ]

From 993a8a3774ac43dc5232ed481eb18ce1c777f8b1 Mon Sep 17 00:00:00 2001
From: Milosz Lagan <milosz.lagan@husarion.com>
Date: Thu, 21 Nov 2024 15:18:05 +0000
Subject: [PATCH 088/100] Change lights BT param types

---
 husarion_ugv_manager/src/lights_manager_node.cpp | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/husarion_ugv_manager/src/lights_manager_node.cpp b/husarion_ugv_manager/src/lights_manager_node.cpp
index 4cdd5484..8d918272 100644
--- a/husarion_ugv_manager/src/lights_manager_node.cpp
+++ b/husarion_ugv_manager/src/lights_manager_node.cpp
@@ -107,11 +107,13 @@ void LightsManagerNode::RegisterBehaviorTree()
 std::map<std::string, std::any> LightsManagerNode::CreateLightsInitialBlackboard()
 {
   update_charging_anim_step_ = this->params_.battery.charging_anim_step;
-  const double critical_battery_anim_period = this->params_.battery.anim_period.critical;
-  const double critical_battery_threshold_percent =
-    this->params_.battery.percent.threshold.critical;
-  const double low_battery_anim_period = this->params_.battery.anim_period.low;
-  const double low_battery_threshold_percent = this->params_.battery.percent.threshold.low;
+  const float critical_battery_anim_period =
+    static_cast<float>(this->params_.battery.anim_period.critical);
+  const float critical_battery_threshold_percent =
+    static_cast<float>(this->params_.battery.percent.threshold.critical);
+  const float low_battery_anim_period = static_cast<float>(this->params_.battery.anim_period.low);
+  const float low_battery_threshold_percent =
+    static_cast<float>(this->params_.battery.percent.threshold.low);
 
   const std::string undefined_charging_anim_percent = "";
   const int undefined_anim_id = -1;

From 93b18fa3d56fe9bd4b48ef7dd99fc99c4b393f53 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mi=C5=82osz=20=C5=81agan?= <milosz.lagan@husarion.com>
Date: Fri, 22 Nov 2024 09:35:38 +0100
Subject: [PATCH 089/100] Change param type

Co-authored-by: Jakub Delicat <109142865+delihus@users.noreply.github.com>
---
 husarion_ugv_manager/src/lights_manager_node.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/husarion_ugv_manager/src/lights_manager_node.cpp b/husarion_ugv_manager/src/lights_manager_node.cpp
index 8d918272..289a2f54 100644
--- a/husarion_ugv_manager/src/lights_manager_node.cpp
+++ b/husarion_ugv_manager/src/lights_manager_node.cpp
@@ -70,7 +70,7 @@ void LightsManagerNode::Initialize()
     "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&LightsManagerNode::EStopCB, this, _1));
 
-  const float timer_freq = this->params_.timer_frequency;
+  const double timer_freq = this->params_.timer_frequency;
   const auto timer_period_ms =
     std::chrono::milliseconds(static_cast<unsigned>(1.0f / timer_freq * 1000));
 

From b891a894b1999a8650b9bfdf2f307c726e05d54d Mon Sep 17 00:00:00 2001
From: Milosz Lagan <milosz.lagan@husarion.com>
Date: Fri, 22 Nov 2024 09:05:06 +0000
Subject: [PATCH 090/100] Review changes

---
 .../husarion_ugv_utils/moving_average.hpp     | 19 +++++++++++--------
 .../test/test_moving_average.cpp              |  8 ++++----
 2 files changed, 15 insertions(+), 12 deletions(-)

diff --git a/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
index 03fe6b07..311f214d 100644
--- a/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
+++ b/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
@@ -27,29 +27,32 @@ class MovingAverage
   MovingAverage(const std::size_t window_size = 5, const T initial_value = T(0))
   : window_size_(window_size), initial_value_(initial_value)
   {
+    if (window_size_ == 0) {
+      throw std::invalid_argument("Window size must be greater than 0");
+    }
   }
 
   void Roll(const T value)
   {
-    values_.push_back(value);
+    buffer_.push_back(value);
 
-    if (values_.size() > window_size_) {
-      values_.pop_front();
+    if (buffer_.size() > window_size_) {
+      buffer_.pop_front();
     }
   }
 
-  void Reset() { values_.erase(values_.begin(), values_.end()); }
+  void Reset() { buffer_.erase(buffer_.begin(), buffer_.end()); }
 
   T GetAverage() const
   {
-    if (values_.size() == 0) {
+    if (buffer_.size() == 0) {
       return initial_value_;
     }
 
     T sum = T(0);
 
-    for (const auto & value : values_) {
-      sum += value / static_cast<T>(values_.size());
+    for (const auto & value : buffer_) {
+      sum += value / static_cast<T>(buffer_.size());
     }
 
     return sum;
@@ -57,7 +60,7 @@ class MovingAverage
 
 private:
   const std::size_t window_size_;
-  std::deque<T> values_;
+  std::deque<T> buffer_;
   const T initial_value_;
 };
 
diff --git a/husarion_ugv_utils/test/test_moving_average.cpp b/husarion_ugv_utils/test/test_moving_average.cpp
index e520bc80..9c759980 100644
--- a/husarion_ugv_utils/test/test_moving_average.cpp
+++ b/husarion_ugv_utils/test/test_moving_average.cpp
@@ -59,15 +59,15 @@ TEST(TestMovingAverage, TestHighOverload)
   const std::size_t window_len = 1000;
   husarion_ugv_utils::MovingAverage<double> ma(window_len);
 
-  double sum = 0.0;
+  double avg = 0.0;
   for (std::size_t i = 1; i <= window_len * 10; i++) {
-    sum += double(i) / double(window_len);
+    avg += double(i) / double(window_len);
     ma.Roll(double(i));
 
     // test every 1000 rolls expected average
     if (i % window_len == 0) {
-      EXPECT_LT(sum - ma.GetAverage(), std::numeric_limits<double>::epsilon());
-      sum = 0.0;
+      ASSERT_NEAR(avg, ma.GetAverage(), std::numeric_limits<double>::epsilon());
+      avg = 0.0;
     }
   }
 }

From 72554b836e2d914a25ea8d6fc5e9b83dbb4e72b7 Mon Sep 17 00:00:00 2001
From: Milosz Lagan <milosz.lagan@husarion.com>
Date: Fri, 22 Nov 2024 10:43:13 +0000
Subject: [PATCH 091/100] Use normal accumulation and division for MA
 calculation

---
 .../include/husarion_ugv_utils/moving_average.hpp      | 10 ++++------
 husarion_ugv_utils/test/test_moving_average.cpp        |  9 +++++----
 2 files changed, 9 insertions(+), 10 deletions(-)

diff --git a/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
index 311f214d..fc1d6b4f 100644
--- a/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
+++ b/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
@@ -16,6 +16,7 @@
 #define HUSARION_UGV_UTILS_MOVING_AVERAGE_HPP_
 
 #include <deque>
+#include <numeric>
 
 namespace husarion_ugv_utils
 {
@@ -49,13 +50,10 @@ class MovingAverage
       return initial_value_;
     }
 
-    T sum = T(0);
+    T sum = std::accumulate(buffer_.begin(), buffer_.end(), T(0));
+    T average = sum / static_cast<T>(buffer_.size());
 
-    for (const auto & value : buffer_) {
-      sum += value / static_cast<T>(buffer_.size());
-    }
-
-    return sum;
+    return average;
   }
 
 private:
diff --git a/husarion_ugv_utils/test/test_moving_average.cpp b/husarion_ugv_utils/test/test_moving_average.cpp
index 9c759980..2934f28e 100644
--- a/husarion_ugv_utils/test/test_moving_average.cpp
+++ b/husarion_ugv_utils/test/test_moving_average.cpp
@@ -59,15 +59,16 @@ TEST(TestMovingAverage, TestHighOverload)
   const std::size_t window_len = 1000;
   husarion_ugv_utils::MovingAverage<double> ma(window_len);
 
-  double avg = 0.0;
+  double sum = 0.0;
   for (std::size_t i = 1; i <= window_len * 10; i++) {
-    avg += double(i) / double(window_len);
+    sum += double(i);
     ma.Roll(double(i));
 
     // test every 1000 rolls expected average
     if (i % window_len == 0) {
-      ASSERT_NEAR(avg, ma.GetAverage(), std::numeric_limits<double>::epsilon());
-      avg = 0.0;
+      ASSERT_NEAR(
+        sum / double(window_len), ma.GetAverage(), std::numeric_limits<double>::epsilon());
+      sum = 0.0;
     }
   }
 }

From 1c29f54d7bd429819fc91cb76cf2310af01ab194 Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Mon, 25 Nov 2024 13:22:04 +0000
Subject: [PATCH 092/100] fix example

---
 husarion_ugv_lights/CONFIGURATION.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/husarion_ugv_lights/CONFIGURATION.md b/husarion_ugv_lights/CONFIGURATION.md
index 986aba94..a4d8d6de 100644
--- a/husarion_ugv_lights/CONFIGURATION.md
+++ b/husarion_ugv_lights/CONFIGURATION.md
@@ -96,7 +96,7 @@ user_animations:
       - type: husarion_ugv_lights::ImageAnimation
         segments: all
         animation:
-          image: $(find husarion_ugv_lights)/animations/strip01_red.png
+          image: $(find husarion_ugv_lights)/animations/panther/strip01_red.png
           duration: 2
           repeat: 2
           color: 0xffff00

From 92f56bac212ed9db9faf6a171efcd91fd2f941e4 Mon Sep 17 00:00:00 2001
From: Dawid Kmak <73443304+KmakD@users.noreply.github.com>
Date: Thu, 28 Nov 2024 13:37:46 +0100
Subject: [PATCH 093/100] fix CHRG_DISABLE init state (#447)

---
 .../src/robot_system/gpio/gpio_controller.cpp                  | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp
index f511a9af..bfb8bdc5 100644
--- a/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp
+++ b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp
@@ -212,7 +212,8 @@ std::unordered_map<GPIOPin, bool> GPIOController::QueryControlInterfaceIOStates(
 const std::vector<GPIOInfo> GPIOController::gpio_config_info_storage_ = {
   GPIOInfo{GPIOPin::WATCHDOG, gpiod::line::direction::OUTPUT},
   GPIOInfo{GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT},
-  GPIOInfo{GPIOPin::CHRG_DISABLE, gpiod::line::direction::OUTPUT},
+  GPIOInfo{
+    GPIOPin::CHRG_DISABLE, gpiod::line::direction::OUTPUT, false, gpiod::line::value::ACTIVE},
   GPIOInfo{GPIOPin::DRIVER_EN, gpiod::line::direction::OUTPUT},
   GPIOInfo{GPIOPin::E_STOP_RESET, gpiod::line::direction::INPUT},
   GPIOInfo{GPIOPin::FAN_SW, gpiod::line::direction::OUTPUT},

From 5ff7a67d84106d995cd33cbe3c32d1ade2513bda Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Thu, 28 Nov 2024 13:29:34 +0000
Subject: [PATCH 094/100] change error to warn

---
 .../src/battery_publisher/battery_publisher.cpp               | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp b/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp
index e37fcbfe..74108408 100644
--- a/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp
+++ b/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp
@@ -49,12 +49,12 @@ void BatteryPublisher::Publish()
     this->Update();
     last_battery_info_time_ = GetClock()->now();
   } catch (const std::runtime_error & e) {
-    RCLCPP_ERROR_STREAM_THROTTLE(
+    RCLCPP_WARN_STREAM_THROTTLE(
       GetLogger(), *GetClock(), 1000,
       "An exception occurred while reading battery data: " << e.what());
 
     diagnostic_updater_->broadcast(
-      diagnostic_msgs::msg::DiagnosticStatus::ERROR,
+      diagnostic_msgs::msg::DiagnosticStatus::WARN,
       "Error reading battery data: " + std::string(e.what()));
   }
 

From 450e189e8d4f315dc20f1b90b20de6935b468630 Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Wed, 4 Dec 2024 14:43:30 +0000
Subject: [PATCH 095/100] use ROBOT_MODEL_NAME env

---
 README.md                                           | 2 +-
 husarion_ugv_bringup/launch/bringup.launch.py       | 7 ++-----
 husarion_ugv_controller/launch/controller.launch.py | 5 +----
 husarion_ugv_gazebo/launch/simulate_robot.launch.py | 5 +----
 husarion_ugv_gazebo/launch/spawn_robot.launch.py    | 5 +----
 husarion_ugv_lights/launch/lights.launch.py         | 5 +----
 6 files changed, 7 insertions(+), 22 deletions(-)

diff --git a/README.md b/README.md
index 1b7dee1e..63c8c9ab 100644
--- a/README.md
+++ b/README.md
@@ -104,7 +104,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | ✅   | ✅   | `localization_mode`          | Specifies the localization mode:  <br/>- 'relative' `odometry/filtered` data is relative to the initial position and orientation. <br/>- 'enu' `odometry/filtered` data is relative to initial position and ENU (East North Up) orientation. <br/> ***string:*** `relative` (choices: `relative`, `enu`)           |
 | ✅   | ✅   | `namespace`                  | Add namespace to all launched nodes. <br/> ***string:*** `env(ROBOT_NAMESPACE)`                                                                                                                                                                                                                                    |
 | ✅   | ✅   | `publish_robot_state`        | Whether to publish the default URDF of specified robot. <br/> ***bool:*** `True`                                                                                                                                                                                                                                   |
-| ❌   | ✅   | `robot_model`                | Specify robot model type. <br/> ***string:*** `env(ROBOT_MODEL)` (choices: `lynx`, `panther`)                                                                                                                                                                                                                      |
+| ❌   | ✅   | `robot_model`                | Specify robot model type. <br/> ***string:*** `env(ROBOT_MODEL_NAME)` (choices: `lynx`, `panther`)                                                                                                                                                                                                                      |
 | ✅   | ✅   | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`SafetyBT.btproj`](./husarion_ugv_manager/behavior_trees/SafetyBT.btproj)                                                                                                                    |
 | ✅   | ✅   | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts.yaml`](./husarion_ugv_manager/config/shutdown_hosts.yaml)                                                                                                                                                                |
 | ✅   | ✅   | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                    |
diff --git a/husarion_ugv_bringup/launch/bringup.launch.py b/husarion_ugv_bringup/launch/bringup.launch.py
index 78e52927..1e542143 100644
--- a/husarion_ugv_bringup/launch/bringup.launch.py
+++ b/husarion_ugv_bringup/launch/bringup.launch.py
@@ -14,7 +14,6 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import os
 
 from husarion_ugv_utils.messages import welcome_msg
 from launch import LaunchDescription
@@ -45,12 +44,10 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
-    robot_model_dict = {"LNX": "lynx", "PTH": "panther"}
-    robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
-    robot_model = robot_model_dict[robot_model_env]
+    robot_model_name = EnvironmentVariable(name="ROBOT_MODEL_NAME", default_value="panther")
     robot_serial_no = EnvironmentVariable(name="ROBOT_SERIAL_NO", default_value="----")
     robot_version = EnvironmentVariable(name="ROBOT_VERSION", default_value="1.0")
-    welcome_info = welcome_msg(robot_model, robot_serial_no, robot_version)
+    welcome_info = welcome_msg(robot_model_name, robot_serial_no, robot_version)
 
     controller_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
diff --git a/husarion_ugv_controller/launch/controller.launch.py b/husarion_ugv_controller/launch/controller.launch.py
index 661f67cd..843bbbc6 100644
--- a/husarion_ugv_controller/launch/controller.launch.py
+++ b/husarion_ugv_controller/launch/controller.launch.py
@@ -39,15 +39,12 @@ def generate_launch_description():
     )
 
     robot_model = LaunchConfiguration("robot_model")
-    robot_model_dict = {"LNX": "lynx", "PTH": "panther"}
-    robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
-
     robot_description_pkg = PythonExpression(["'", robot_model, "_description'"])
     robot_description_common_dir = PathJoinSubstitution(["/config", robot_description_pkg])
 
     declare_robot_model_arg = DeclareLaunchArgument(
         "robot_model",
-        default_value=robot_model_dict[robot_model_env],
+        default_value=EnvironmentVariable(name="ROBOT_MODEL_NAME", default_value="panther"),
         description="Specify robot model",
         choices=["lynx", "panther"],
     )
diff --git a/husarion_ugv_gazebo/launch/simulate_robot.launch.py b/husarion_ugv_gazebo/launch/simulate_robot.launch.py
index db367687..a13a0115 100644
--- a/husarion_ugv_gazebo/launch/simulate_robot.launch.py
+++ b/husarion_ugv_gazebo/launch/simulate_robot.launch.py
@@ -14,7 +14,6 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import os
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
@@ -94,11 +93,9 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
-    robot_model_dict = {"LNX": "lynx", "PTH": "panther"}
-    robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
     declare_robot_model_arg = DeclareLaunchArgument(
         "robot_model",
-        default_value=robot_model_dict[robot_model_env],
+        default_value=EnvironmentVariable(name="ROBOT_MODEL_NAME", default_value="panther"),
         description="Specify robot model",
         choices=["lynx", "panther"],
     )
diff --git a/husarion_ugv_gazebo/launch/spawn_robot.launch.py b/husarion_ugv_gazebo/launch/spawn_robot.launch.py
index 39a47a2c..f3013e13 100644
--- a/husarion_ugv_gazebo/launch/spawn_robot.launch.py
+++ b/husarion_ugv_gazebo/launch/spawn_robot.launch.py
@@ -14,7 +14,6 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import os
 
 from husarion_ugv_utils.messages import welcome_msg
 from launch import LaunchDescription
@@ -40,11 +39,9 @@ def generate_launch_description():
     )
 
     robot_model = LaunchConfiguration("robot_model")
-    robot_model_dict = {"LNX": "lynx", "PTH": "panther"}
-    robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
     declare_robot_model_arg = DeclareLaunchArgument(
         "robot_model",
-        default_value=robot_model_dict[robot_model_env],
+        default_value=EnvironmentVariable(name="ROBOT_MODEL_NAME", default_value="panther"),
         description="Specify robot model",
         choices=["lynx", "panther"],
     )
diff --git a/husarion_ugv_lights/launch/lights.launch.py b/husarion_ugv_lights/launch/lights.launch.py
index 8fd1f466..27023d82 100644
--- a/husarion_ugv_lights/launch/lights.launch.py
+++ b/husarion_ugv_lights/launch/lights.launch.py
@@ -15,7 +15,6 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import os
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, Shutdown
@@ -51,11 +50,9 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
-    robot_model_dict = {"LNX": "lynx", "PTH": "panther"}
-    robot_model_env = os.environ.get("ROBOT_MODEL", default="PTH")
     declare_robot_model_arg = DeclareLaunchArgument(
         "robot_model",
-        default_value=robot_model_dict[robot_model_env],
+        default_value=EnvironmentVariable(name="ROBOT_MODEL_NAME", default_value="panther"),
         description="Specify robot model",
         choices=["lynx", "panther"],
     )

From f09439a4213938f1c6441f91238e35a9ed39e80d Mon Sep 17 00:00:00 2001
From: Dawid Kmak <73443304+KmakD@users.noreply.github.com>
Date: Thu, 5 Dec 2024 11:03:05 +0100
Subject: [PATCH 096/100] Better battery estimation (#452)

* better battery estimation

* update README
---
 husarion_ugv_battery/README.md                     | 14 +++++++-------
 .../husarion_ugv_battery/battery/adc_battery.hpp   |  7 ++++++-
 .../husarion_ugv_battery/battery/battery.hpp       | 11 ++++++-----
 husarion_ugv_battery/src/battery/adc_battery.cpp   |  4 ++--
 .../test/battery/test_adc_battery.cpp              |  5 +++++
 husarion_ugv_battery/test/battery/test_battery.cpp | 11 +++++++----
 .../test/test_battery_driver_node_adc_dual.cpp     |  4 ++--
 .../test/test_battery_driver_node_adc_single.cpp   |  4 ++--
 .../test/utils/test_battery_driver_node.hpp        |  5 ++---
 9 files changed, 39 insertions(+), 26 deletions(-)

diff --git a/husarion_ugv_battery/README.md b/husarion_ugv_battery/README.md
index e952f05d..263ea252 100644
--- a/husarion_ugv_battery/README.md
+++ b/husarion_ugv_battery/README.md
@@ -33,11 +33,11 @@ Publishes battery state read from ADC unit.
 
 #### Parameters
 
-- `~/adc/device0` [*string*, default: **/sys/bus/iio/devices/iio:device0**]: ADC number 0 IIO device.
-- `~/adc/device1` [*string*, default: **/sys/bus/iio/devices/iio:device1**]: ADC number 1 IIO device.
-- `~/adc/ma_window_len/charge` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery charge readings.
-- `~/adc/ma_window_len/temp` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery temperature readings.
+- `~/adc.device0` [*string*, default: **/sys/bus/iio/devices/iio:device0**]: ADC number 0 IIO device.
+- `~/adc.device1` [*string*, default: **/sys/bus/iio/devices/iio:device1**]: ADC number 1 IIO device.
+- `~/adc.ma_window_len.charge` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery charge readings.
+- `~/adc.ma_window_len.temp` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery temperature readings.
 - `~/battery_timeout` [*float*, default: **1.0**]: Specifies the timeout in seconds. If the node fails to read battery data exceeding this duration, the node will publish an unknown battery state.
-- `~/ma_window_len/voltage` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery voltage readings.
-- `~/ma_window_len/current` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery current readings.
-- `~/roboteq/driver_state_timeout` [*float*, default: **0.2**]: Specifies timeout in seconds after which driver state messages will be considered old (deprecated).
+- `~/ma_window_len.voltage` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery voltage readings.
+- `~/ma_window_len.current` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery current readings.
+- `~/roboteq.driver_state_timeout` [*float*, default: **0.2**]: Specifies timeout in seconds after which driver state messages will be considered old (deprecated).
diff --git a/husarion_ugv_battery/include/husarion_ugv_battery/battery/adc_battery.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery/adc_battery.hpp
index 94dd428d..20b39591 100644
--- a/husarion_ugv_battery/include/husarion_ugv_battery/battery/adc_battery.hpp
+++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery/adc_battery.hpp
@@ -77,7 +77,12 @@ class ADCBattery : public Battery
   static constexpr float kR1 = 10000.0;
   static constexpr float kR0 = 10000.0;
   static constexpr float kUSupply = 3.28;
-  static constexpr float kKelvinToCelciusOffset = 273.15;
+  static constexpr float kKelvinToCelsiusOffset = 273.15;
+
+  // Threshold used for diagnostics. At this voltage level, and below, the battery shall draw a
+  // significant current from the charger if the charger is connected. If not, the charging circuit
+  // may be broken.
+  static constexpr float kBatteryCCCheckTresh = 41.2;
 
   float voltage_raw_;
   float current_raw_;
diff --git a/husarion_ugv_battery/include/husarion_ugv_battery/battery/battery.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery/battery.hpp
index 7e461cfa..94392164 100644
--- a/husarion_ugv_battery/include/husarion_ugv_battery/battery/battery.hpp
+++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery/battery.hpp
@@ -57,14 +57,14 @@ class Battery
 
   float GetBatteryPercent(const float voltage) const
   {
-    for (int i = 0; i < 4; i++) {
+    for (int i = 0; i < 5; i++) {
       if (voltage > battery_approx_ranges[i]) {
         return std::clamp(
           (battery_approx_a_values[i] * voltage + battery_approx_b_values[i]) / 100, 0.0f, 1.0f);
       }
     }
     return std::clamp(
-      (battery_approx_a_values[4] * voltage + battery_approx_b_values[4]) / 100, 0.0f, 1.0f);
+      (battery_approx_a_values[5] * voltage + battery_approx_b_values[5]) / 100, 0.0f, 1.0f);
   }
 
   void ResetBatteryMsgs(const rclcpp::Time & header_stamp)
@@ -117,9 +117,10 @@ class Battery
   static constexpr float kDesignedCapacity = 20.0;
   static constexpr std::string_view kLocation = "user_compartment";
 
-  static constexpr float battery_approx_ranges[4] = {41.25, 37, 35, 33.7};
-  static constexpr float battery_approx_a_values[5] = {1.733, 9.153, 19.8, 10.538, 0.989};
-  static constexpr float battery_approx_b_values[5] = {27.214, -278.861, -672.6, -351.47, -29.67};
+  static constexpr float battery_approx_ranges[5] = {41.25, 37.0, 36.0, 35.0, 33.7};
+  static constexpr float battery_approx_a_values[6] = {8.665, 9.153, 19.8, 22.84, 10.538, 0.989};
+  static constexpr float battery_approx_b_values[6] = {-258.73, -278.861, -672.6,
+                                                       -782.04, -351.47,  -29.669};
 
   std::string error_msg_;
   BatteryStateMsg battery_state_;
diff --git a/husarion_ugv_battery/src/battery/adc_battery.cpp b/husarion_ugv_battery/src/battery/adc_battery.cpp
index cd13f25a..68db4d73 100644
--- a/husarion_ugv_battery/src/battery/adc_battery.cpp
+++ b/husarion_ugv_battery/src/battery/adc_battery.cpp
@@ -107,7 +107,7 @@ float ADCBattery::ADCToBatteryTemp(const float adc_data) const
 
   const float R_therm = (adc_data * kR1) / (kUSupply - adc_data);
   return (kTempCoeffA * kTempCoeffB / (kTempCoeffA * logf(R_therm / kR0) + kTempCoeffB)) -
-         kKelvinToCelciusOffset;
+         kKelvinToCelsiusOffset;
 }
 
 void ADCBattery::UpdateBatteryMsgs(const rclcpp::Time & header_stamp, const bool charger_connected)
@@ -167,7 +167,7 @@ std::uint8_t ADCBattery::GetBatteryStatus(const float charge, const bool charger
   if (charger_connected) {
     if (fabs(battery_state_.percentage - 1.0f) < std::numeric_limits<float>::epsilon()) {
       return BatteryStateMsg::POWER_SUPPLY_STATUS_FULL;
-    } else if (charge > kChargingCurrentTresh) {
+    } else if (charge > kChargingCurrentTresh || battery_state_.voltage > kBatteryCCCheckTresh) {
       return BatteryStateMsg::POWER_SUPPLY_STATUS_CHARGING;
     } else {
       return BatteryStateMsg::POWER_SUPPLY_STATUS_NOT_CHARGING;
diff --git a/husarion_ugv_battery/test/battery/test_adc_battery.cpp b/husarion_ugv_battery/test/battery/test_adc_battery.cpp
index 9486c85e..13a5680f 100644
--- a/husarion_ugv_battery/test/battery/test_adc_battery.cpp
+++ b/husarion_ugv_battery/test/battery/test_adc_battery.cpp
@@ -253,6 +253,11 @@ TEST_F(TestADCBattery, BatteryMsgStatusCharging)
   UpdateBattery(1.5, 0.01, 0.98, 0.5, true);
 
   EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_STATUS_CHARGING, battery_state_.power_supply_status);
+
+  // Small charge, but voltage over 41.2 V
+  UpdateBattery(1.65, 0.01, 0.98, 0.04, true);
+
+  EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_STATUS_CHARGING, battery_state_.power_supply_status);
 }
 
 TEST_F(TestADCBattery, BatteryMsgStatusNotCharging)
diff --git a/husarion_ugv_battery/test/battery/test_battery.cpp b/husarion_ugv_battery/test/battery/test_battery.cpp
index 9b2d12c0..62195ca3 100644
--- a/husarion_ugv_battery/test/battery/test_battery.cpp
+++ b/husarion_ugv_battery/test/battery/test_battery.cpp
@@ -109,10 +109,13 @@ TEST_F(TestBattery, GetErrorMsg)
 
 TEST_F(TestBattery, GetBatteryPercent)
 {
-  EXPECT_FLOAT_EQ(0.0, battery_->GetBatteryPercent(30.0f));
-  EXPECT_FLOAT_EQ(0.019780006, battery_->GetBatteryPercent(32.0f));
-  EXPECT_FLOAT_EQ(0.68953001, battery_->GetBatteryPercent(38.0f));
-  EXPECT_FLOAT_EQ(1.0, battery_->GetBatteryPercent(42.0f));
+  EXPECT_NEAR(0.0, battery_->GetBatteryPercent(30.0f), 1e-5);
+  EXPECT_NEAR(0.01979, battery_->GetBatteryPercent(32.0f), 1e-6);
+  EXPECT_NEAR(0.06822, battery_->GetBatteryPercent(34.0f), 1e-6);
+  EXPECT_NEAR(0.2878, battery_->GetBatteryPercent(35.5f), 1e-6);
+  EXPECT_NEAR(0.501, battery_->GetBatteryPercent(36.5f), 1e-6);
+  EXPECT_NEAR(0.68953, battery_->GetBatteryPercent(38.0f), 1e-6);
+  EXPECT_FLOAT_EQ(1.0, battery_->GetBatteryPercent(41.4f));
   EXPECT_FLOAT_EQ(1.0, battery_->GetBatteryPercent(45.0f));
 }
 
diff --git a/husarion_ugv_battery/test/test_battery_driver_node_adc_dual.cpp b/husarion_ugv_battery/test/test_battery_driver_node_adc_dual.cpp
index 2718407a..167035ce 100644
--- a/husarion_ugv_battery/test/test_battery_driver_node_adc_dual.cpp
+++ b/husarion_ugv_battery/test/test_battery_driver_node_adc_dual.cpp
@@ -38,8 +38,8 @@ TEST_F(TestBatteryNodeADCDual, BatteryValues)
   EXPECT_FLOAT_EQ(35.05957, battery_state_->voltage);
   EXPECT_FLOAT_EQ(2.02, battery_state_->current);
   EXPECT_FLOAT_EQ(26.094543, battery_state_->temperature);
-  EXPECT_FLOAT_EQ(0.21579468, battery_state_->percentage);
-  EXPECT_FLOAT_EQ(8.6317873, battery_state_->charge);
+  EXPECT_FLOAT_EQ(0.18720642, battery_state_->percentage);
+  EXPECT_FLOAT_EQ(7.4882565, battery_state_->charge);
 
   // If both batteries have the same reading values they should have equal values
   EXPECT_FLOAT_EQ(battery_1_state_->voltage, battery_2_state_->voltage);
diff --git a/husarion_ugv_battery/test/test_battery_driver_node_adc_single.cpp b/husarion_ugv_battery/test/test_battery_driver_node_adc_single.cpp
index 8c0ec498..8ad638e2 100644
--- a/husarion_ugv_battery/test/test_battery_driver_node_adc_single.cpp
+++ b/husarion_ugv_battery/test/test_battery_driver_node_adc_single.cpp
@@ -43,8 +43,8 @@ TEST_F(TestBatteryNodeADCSingle, BatteryValues)
   EXPECT_FLOAT_EQ(35.05957, battery_state_->voltage);
   EXPECT_FLOAT_EQ(2.01, battery_state_->current);
   EXPECT_FLOAT_EQ(26.094543, battery_state_->temperature);
-  EXPECT_FLOAT_EQ(0.21579468, battery_state_->percentage);
-  EXPECT_FLOAT_EQ(4.3158937, battery_state_->charge);
+  EXPECT_FLOAT_EQ(0.18720642, battery_state_->percentage);
+  EXPECT_FLOAT_EQ(3.7441282, battery_state_->charge);
 
   // For single battery if readings stay the same values of battery_1 and battery should be the same
   EXPECT_FLOAT_EQ(battery_1_state_->voltage, battery_state_->voltage);
diff --git a/husarion_ugv_battery/test/utils/test_battery_driver_node.hpp b/husarion_ugv_battery/test/utils/test_battery_driver_node.hpp
index 319dfd6d..7bb73c3a 100644
--- a/husarion_ugv_battery/test/utils/test_battery_driver_node.hpp
+++ b/husarion_ugv_battery/test/utils/test_battery_driver_node.hpp
@@ -71,9 +71,8 @@ TestBatteryNode::TestBatteryNode(const bool use_adc_battery, const bool dual_bat
     device0_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice0;
     device1_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice1;
 
-    params.push_back(rclcpp::Parameter("adc/device0", device0_path_.string()));
-    params.push_back(rclcpp::Parameter("adc/device1", device1_path_.string()));
-    params.push_back(rclcpp::Parameter("adc/path", testing::TempDir()));
+    params.push_back(rclcpp::Parameter("adc.device0", device0_path_.string()));
+    params.push_back(rclcpp::Parameter("adc.device1", device1_path_.string()));
 
     // Create the device0 and device1 directories if they do not exist
     std::filesystem::create_directory(device0_path_);

From b01865e55469d2338f5a96484e1828b944ee647e Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Thu, 5 Dec 2024 13:30:24 +0000
Subject: [PATCH 097/100] fix tests

---
 husarion_ugv_lights/config/user_animations.yaml       |  4 +++-
 husarion_ugv_manager/src/lights_manager_node.cpp      |  1 -
 husarion_ugv_manager/src/safety_manager_node.cpp      |  3 +--
 .../test/test_lights_behavior_tree.cpp                | 11 ++++++++---
 .../test/test_safety_behavior_tree.cpp                |  6 ++++++
 5 files changed, 18 insertions(+), 7 deletions(-)

diff --git a/husarion_ugv_lights/config/user_animations.yaml b/husarion_ugv_lights/config/user_animations.yaml
index 01771347..0d31ca44 100644
--- a/husarion_ugv_lights/config/user_animations.yaml
+++ b/husarion_ugv_lights/config/user_animations.yaml
@@ -4,7 +4,9 @@
 #
 # For more examples and detailed documentation, please visit:
 # https://github.com/husarion/panther_ros/blob/ros2/panther_lights/CONFIGURATION.md#defining-animations
-#
+
+user_animations: []
+
 # Example including a simple custom animation:
 #
 # user_animations:
diff --git a/husarion_ugv_manager/src/lights_manager_node.cpp b/husarion_ugv_manager/src/lights_manager_node.cpp
index 289a2f54..2238ded1 100644
--- a/husarion_ugv_manager/src/lights_manager_node.cpp
+++ b/husarion_ugv_manager/src/lights_manager_node.cpp
@@ -22,7 +22,6 @@
 #include <string>
 #include <vector>
 
-#include "ament_index_cpp/get_package_share_directory.hpp"
 #include "behaviortree_ros2/ros_node_params.hpp"
 #include "rclcpp/rclcpp.hpp"
 
diff --git a/husarion_ugv_manager/src/safety_manager_node.cpp b/husarion_ugv_manager/src/safety_manager_node.cpp
index 63965f4f..33ede2c0 100644
--- a/husarion_ugv_manager/src/safety_manager_node.cpp
+++ b/husarion_ugv_manager/src/safety_manager_node.cpp
@@ -24,7 +24,6 @@
 #include <utility>
 #include <vector>
 
-#include "ament_index_cpp/get_package_share_directory.hpp"
 #include "behaviortree_ros2/ros_node_params.hpp"
 #include "rclcpp/rclcpp.hpp"
 
@@ -138,7 +137,7 @@ std::map<std::string, std::any> SafetyManagerNode::CreateSafetyInitialBlackboard
   const double cpu_fan_off_temp = this->params_.cpu.temp.fan_off;
   const double driver_fan_on_temp = this->params_.driver.temp.fan_on;
   const double driver_fan_off_temp = this->params_.driver.temp.fan_off;
-  const double fan_turn_off_timeout = this->params_.fan_turn_off_timeout;
+  const float fan_turn_off_timeout = static_cast<float>(this->params_.fan_turn_off_timeout);
 
   const std::map<std::string, std::any> safety_initial_bb = {
     {"CPU_FAN_OFF_TEMP", cpu_fan_off_temp},
diff --git a/husarion_ugv_manager/test/test_lights_behavior_tree.cpp b/husarion_ugv_manager/test/test_lights_behavior_tree.cpp
index 4a9f342d..e44285a6 100644
--- a/husarion_ugv_manager/test/test_lights_behavior_tree.cpp
+++ b/husarion_ugv_manager/test/test_lights_behavior_tree.cpp
@@ -22,6 +22,7 @@
 
 #include "gtest/gtest.h"
 
+#include "ament_index_cpp/get_package_share_directory.hpp"
 #include "behaviortree_cpp/basic_types.h"
 #include "rclcpp/rclcpp.hpp"
 
@@ -107,6 +108,10 @@ TestLightsBehaviorTree::TestLightsBehaviorTree()
 
 std::vector<rclcpp::Parameter> TestLightsBehaviorTree::CreateTestParameters() const
 {
+  const auto panther_manager_pkg_path =
+    ament_index_cpp::get_package_share_directory("husarion_ugv_manager");
+  const std::string bt_project_path = panther_manager_pkg_path + "/behavior_trees/LightsBT.btproj";
+
   std::vector<std::string> plugin_libs;
   plugin_libs.push_back("tick_after_timeout_bt_node");
 
@@ -114,11 +119,11 @@ std::vector<rclcpp::Parameter> TestLightsBehaviorTree::CreateTestParameters() co
   ros_plugin_libs.push_back("call_set_led_animation_service_bt_node");
 
   std::vector<rclcpp::Parameter> params;
+  params.push_back(rclcpp::Parameter("bt_project_path", bt_project_path));
   params.push_back(rclcpp::Parameter("plugin_libs", plugin_libs));
   params.push_back(rclcpp::Parameter("ros_plugin_libs", ros_plugin_libs));
-  params.push_back(rclcpp::Parameter("battery.animation_period.low", kLowBatteryAnimPeriod));
-  params.push_back(
-    rclcpp::Parameter("battery.animation_period.critical", kCriticalBatteryAnimPeriod));
+  params.push_back(rclcpp::Parameter("battery.anim_period.low", kLowBatteryAnimPeriod));
+  params.push_back(rclcpp::Parameter("battery.anim_period.critical", kCriticalBatteryAnimPeriod));
 
   return params;
 }
diff --git a/husarion_ugv_manager/test/test_safety_behavior_tree.cpp b/husarion_ugv_manager/test/test_safety_behavior_tree.cpp
index 6f9540bf..9e7dc51d 100644
--- a/husarion_ugv_manager/test/test_safety_behavior_tree.cpp
+++ b/husarion_ugv_manager/test/test_safety_behavior_tree.cpp
@@ -22,6 +22,7 @@
 
 #include "gtest/gtest.h"
 
+#include "ament_index_cpp/get_package_share_directory.hpp"
 #include "behaviortree_cpp/basic_types.h"
 #include "rclcpp/rclcpp.hpp"
 
@@ -126,6 +127,10 @@ TestSafetyBehaviorTree::~TestSafetyBehaviorTree() { rclcpp::shutdown(); }
 
 std::vector<rclcpp::Parameter> TestSafetyBehaviorTree::CreateTestParameters() const
 {
+  const auto panther_manager_pkg_path =
+    ament_index_cpp::get_package_share_directory("husarion_ugv_manager");
+  const std::string bt_project_path = panther_manager_pkg_path + "/behavior_trees/SafetyBT.btproj";
+
   std::vector<std::string> plugin_libs;
   plugin_libs.push_back("tick_after_timeout_bt_node");
   plugin_libs.push_back("shutdown_single_host_bt_node");
@@ -137,6 +142,7 @@ std::vector<rclcpp::Parameter> TestSafetyBehaviorTree::CreateTestParameters() co
   ros_plugin_libs.push_back("call_trigger_service_bt_node");
 
   std::vector<rclcpp::Parameter> params;
+  params.push_back(rclcpp::Parameter("bt_project_path", bt_project_path));
   params.push_back(rclcpp::Parameter("plugin_libs", plugin_libs));
   params.push_back(rclcpp::Parameter("ros_plugin_libs", ros_plugin_libs));
   params.push_back(rclcpp::Parameter("fan_turn_off_timeout", kFanTurnOffTimeout));

From 3045193fcd4de8b3d273520f2809cee9f2d23818 Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Fri, 6 Dec 2024 07:42:15 +0000
Subject: [PATCH 098/100] change panther_msgs branch

---
 husarion_ugv/hardware_deps.repos | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/husarion_ugv/hardware_deps.repos b/husarion_ugv/hardware_deps.repos
index 171ec29f..eceb83d6 100644
--- a/husarion_ugv/hardware_deps.repos
+++ b/husarion_ugv/hardware_deps.repos
@@ -10,7 +10,7 @@ repositories:
   panther_msgs:
     type: git
     url: https://github.com/husarion/panther_msgs.git
-    version: ros2-lynx-devel
+    version: ros2-devel
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git

From 5855b70842174575ed9d71d9c32364305a6f8976 Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Fri, 6 Dec 2024 07:43:09 +0000
Subject: [PATCH 099/100] change panther_msgs branch

---
 husarion_ugv/simulation_deps.repos | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/husarion_ugv/simulation_deps.repos b/husarion_ugv/simulation_deps.repos
index a754ab3e..e6c81113 100644
--- a/husarion_ugv/simulation_deps.repos
+++ b/husarion_ugv/simulation_deps.repos
@@ -10,7 +10,7 @@ repositories:
   panther_msgs:
     type: git
     url: https://github.com/husarion/panther_msgs.git
-    version: ros2-lynx-devel
+    version: ros2-devel
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git

From 05aab48967843e38627d417bff829a7266a5c94a Mon Sep 17 00:00:00 2001
From: kmakd <kmakd197@gmail.com>
Date: Fri, 6 Dec 2024 08:19:19 +0000
Subject: [PATCH 100/100] decrease cmd_vel_timeout

---
 husarion_ugv_controller/config/WH01_controller.yaml | 2 +-
 husarion_ugv_controller/config/WH02_controller.yaml | 2 +-
 husarion_ugv_controller/config/WH04_controller.yaml | 2 +-
 husarion_ugv_controller/config/WH05_controller.yaml | 2 +-
 4 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/husarion_ugv_controller/config/WH01_controller.yaml b/husarion_ugv_controller/config/WH01_controller.yaml
index 4187aa50..da21d73e 100644
--- a/husarion_ugv_controller/config/WH01_controller.yaml
+++ b/husarion_ugv_controller/config/WH01_controller.yaml
@@ -55,7 +55,7 @@
 
       enable_odom_tf: false
 
-      cmd_vel_timeout: 0.5
+      cmd_vel_timeout: 0.2
       #publish_limited_velocity: true
       use_stamped_vel: false
 
diff --git a/husarion_ugv_controller/config/WH02_controller.yaml b/husarion_ugv_controller/config/WH02_controller.yaml
index 6d109509..0e6e8ee5 100644
--- a/husarion_ugv_controller/config/WH02_controller.yaml
+++ b/husarion_ugv_controller/config/WH02_controller.yaml
@@ -56,7 +56,7 @@
 
       enable_odom_tf: false
 
-      cmd_vel_timeout: 0.5
+      cmd_vel_timeout: 0.2
       #publish_limited_velocity: true
       use_stamped_vel: false
 
diff --git a/husarion_ugv_controller/config/WH04_controller.yaml b/husarion_ugv_controller/config/WH04_controller.yaml
index b13bbd11..2ae5cb34 100644
--- a/husarion_ugv_controller/config/WH04_controller.yaml
+++ b/husarion_ugv_controller/config/WH04_controller.yaml
@@ -56,7 +56,7 @@
 
       enable_odom_tf: false
 
-      cmd_vel_timeout: 0.5
+      cmd_vel_timeout: 0.2
       #publish_limited_velocity: true
       use_stamped_vel: false
 
diff --git a/husarion_ugv_controller/config/WH05_controller.yaml b/husarion_ugv_controller/config/WH05_controller.yaml
index 7499d0fb..e9cfd598 100644
--- a/husarion_ugv_controller/config/WH05_controller.yaml
+++ b/husarion_ugv_controller/config/WH05_controller.yaml
@@ -55,7 +55,7 @@
 
       enable_odom_tf: false
 
-      cmd_vel_timeout: 0.5
+      cmd_vel_timeout: 0.2
       #publish_limited_velocity: true
       use_stamped_vel: false