diff --git a/README.md b/README.md
index 23e0a0b..e3dec43 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,4 @@
 # robot_base
-The challange in the 4x4 wheeldrive is the 2 USB connections to be made in the hardawre interface 
-this is WIP 
+The chalenge in the 4x4 wheeldrive is the 2 USB ports that spawn to run in sync with eachother - WIP 
 
 # robot_base
diff --git a/hoverboard_hardware_interface/include/hoverboard_hardware_interface/hoverboard_hardware_interface.hpp b/hoverboard_hardware_interface/include/hoverboard_hardware_interface/hoverboard_hardware_interface.hpp
index 725cc80..351e449 100644
--- a/hoverboard_hardware_interface/include/hoverboard_hardware_interface/hoverboard_hardware_interface.hpp
+++ b/hoverboard_hardware_interface/include/hoverboard_hardware_interface/hoverboard_hardware_interface.hpp
@@ -29,20 +29,23 @@ namespace hoverboard_hardware_interface
     class HoverboardHardwareInterface : public hardware_interface::SystemInterface
     {
         struct HardwareConfig
-        {
-            std::string leftWheelJointName = "left_front_wheel_joint";
-            std::string rightWheelJointName = "right_front_wheel_joint";
+{
+    std::string frontLeftWheelJointName = "left_front_wheel_joint";
+    std::string frontRightWheelJointName = "right_front_wheel_joint";
+    std::string backLeftWheelJointName = "left_back_wheel_joint";
+    std::string backRightWheelJointName = "right_back_wheel_joint";
 
-            float loopRate = 30.0;
-            int encoderTicksPerRevolution = 1024;
-        };
+    float loopRate = 30.0;
+    int encoderTicksPerRevolution = 1024;
+};
 
-        struct SerialPortConfig
-        {
-            std::string device = "/dev/ttyUSB0";
-            int baudRate = 115200;
-            int timeout = 1000;
-        };
+struct SerialPortConfig
+{
+    std::string frontDevice = "/dev/ttyUSB0";
+    std::string backDevice = "/dev/ttyUSB1";
+    int baudRate = 115200;
+    int timeout = 1000;
+};
 
     public:
         RCLCPP_SHARED_PTR_DEFINITIONS(HoverboardHardwareInterface)
@@ -67,17 +70,20 @@ namespace hoverboard_hardware_interface
 
         void motorWheelFeedbackCallback(MotorWheelFeedback);
 
-    private:
+private:
+    SerialPortService frontSerialPortService;
+    SerialPortService backSerialPortService;
 
-        SerialPortService serialPortService;
+    HardwareConfig hardwareConfig;
+    SerialPortConfig serialPortConfig;
 
-        HardwareConfig hardwareConfig;
-        SerialPortConfig serialPortConfig;
+    MotorWheel frontLeftWheel;
+    MotorWheel frontRightWheel;
+    MotorWheel backLeftWheel;
+    MotorWheel backRightWheel;
 
-        MotorWheel leftWheel;
-        MotorWheel rightWheel;
+    bool connect();
+    bool disconnect();
 
-        bool connect();
-        bool disconnect();
     };
 }
diff --git a/hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_protocol.hpp b/hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_protocol.hpp
index 0227cdd..58ee1a3 100644
--- a/hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_protocol.hpp
+++ b/hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_protocol.hpp
@@ -18,14 +18,15 @@
 
 #define HEAD_FRAME 0xABCD
 
-// enum class MOTOR_STATES {
-//     UNOCUPPIED = 0b00,
-//     RUN = 0b01,
-//     BRAKE = 0b11,
-//     LOCK_SHAFT = 0b10,
-// };
+enum class MOTOR_STATES {
+    UNOCUPPIED = 0b00,
+    RUN = 0b01,
+    BRAKE = 0b11,
+    LOCK_SHAFT = 0b10,
+};
 
 typedef struct {
+    uint8_t hoverboard_id; // Identifier for the hoverboard
     uint16_t head;
     int16_t  command1;
     int16_t  command2;
@@ -40,8 +41,10 @@ typedef struct {
 } MotorWheelFeedback;
 
 typedef struct {
+    uint8_t hoverboard_id; // Identifier for the hoverboard
     uint16_t head = HEAD_FRAME;
     int16_t  steer;
     int16_t  speed;
     uint16_t checksum;
 } MotorWheelDriveControl;
+
diff --git a/hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_service.hpp b/hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_service.hpp
index dad4631..15266e0 100644
--- a/hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_service.hpp
+++ b/hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_service.hpp
@@ -1,26 +1,9 @@
-// Copyright 2023 Robert Gruberski (Viola Robotics Sp. z o.o. Poland)
-//
-// 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.
-
 #pragma once
 
 #include <string>
-
 #include <boost/asio.hpp>
 #include <boost/thread.hpp>
-
 #include "rclcpp/rclcpp.hpp"
-
 #include "serial_port_protocol.hpp"
 
 #define SERIAL_PORT_READ_BUF_SIZE 256
@@ -31,8 +14,7 @@ namespace hoverboard_hardware_interface
 {
     class SerialPortService
     {
-        public:
-
+    public:
         SerialPortService() = default;
 
         bool connect(const std::string &serial_device, int baud_rate, int timeout);
@@ -45,10 +27,10 @@ namespace hoverboard_hardware_interface
 
         void BindMotorWheelFeedbackCallback(std::function<void(MotorWheelFeedback)>);
 
-        private:
-
+    private:
         boost::asio::io_service io_service;
-        serial_port_ptr port;
+        serial_port_ptr port; // Declaration of 'port' as a member variable
+
         boost::mutex mutex;
 
         uint16_t head_frame = 0;
@@ -66,4 +48,5 @@ namespace hoverboard_hardware_interface
 
         MotorWheelFeedback motorWheelFeedback {};
     };
-}
+} // namespace hoverboard_hardware_interface
+
diff --git a/hoverboard_hardware_interface/src/hoverboard_hardware_interface.cpp b/hoverboard_hardware_interface/src/hoverboard_hardware_interface.cpp
index 576f024..63036f5 100644
--- a/hoverboard_hardware_interface/src/hoverboard_hardware_interface.cpp
+++ b/hoverboard_hardware_interface/src/hoverboard_hardware_interface.cpp
@@ -22,19 +22,22 @@ namespace hoverboard_hardware_interface
             return hardware_interface::CallbackReturn::ERROR;
         }
 
-        hardwareConfig.leftWheelJointName = info.hardware_parameters.at("left_front_wheel_joint_name");
-        hardwareConfig.rightWheelJointName = info.hardware_parameters.at("right_front_wheel_joint_name");
+        hardwareConfig.frontLeftWheelJointName = info.hardware_parameters.at("left_front_wheel_joint_name");
+        hardwareConfig.frontRightWheelJointName = info.hardware_parameters.at("right_front_wheel_joint_name");
+        hardwareConfig.backLeftWheelJointName = info.hardware_parameters.at("left_back_wheel_joint_name");
+        hardwareConfig.backRightWheelJointName = info.hardware_parameters.at("right_back_wheel_joint_name");
         hardwareConfig.loopRate = std::stof(info.hardware_parameters.at("loop_rate"));
-        // hardwareConfig.encoderTicksPerRevolution = std::stoi(info.hardware_parameters.at("encoder_ticks_per_revolution"));
+        //hardwareConfig.encoderTicksPerRevolution = std::stoi(info.hardware_parameters.at("encoder_ticks_per_revolution"));
 
-        serialPortConfig.device = info.hardware_parameters.at("device");
+        serialPortConfig.frontDevice = info.hardware_parameters.at("front_device");
+        serialPortConfig.backDevice = info.hardware_parameters.at("back_device");
         serialPortConfig.baudRate = std::stoi(info.hardware_parameters.at("baud_rate"));
         serialPortConfig.timeout = std::stoi(info.hardware_parameters.at("timeout"));
 
-        leftWheel = MotorWheel(info.hardware_parameters.at("left_front_wheel_joint_name"), 
-                                std::stoi(info.hardware_parameters.at("encoder_ticks_per_revolution")));
-        rightWheel = MotorWheel(info.hardware_parameters.at("right_front_wheel_joint_name"), 
-                                std::stoi(info.hardware_parameters.at("encoder_ticks_per_revolution")));
+        frontLeftWheel = MotorWheel(hardwareConfig.frontLeftWheelJointName, hardwareConfig.encoderTicksPerRevolution);
+        frontRightWheel = MotorWheel(hardwareConfig.frontRightWheelJointName, hardwareConfig.encoderTicksPerRevolution);
+        backLeftWheel = MotorWheel(hardwareConfig.backLeftWheelJointName, hardwareConfig.encoderTicksPerRevolution);
+        backRightWheel = MotorWheel(hardwareConfig.backRightWheelJointName, hardwareConfig.encoderTicksPerRevolution);
 
         for (const hardware_interface::ComponentInfo & joint : info.joints)
         {
@@ -51,7 +54,7 @@ namespace hoverboard_hardware_interface
             {
                 RCLCPP_FATAL(
                     rclcpp::get_logger("HoverboardHardwareInterface"),
-                    "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
+                    "Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(),
                     joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
                 
                 return hardware_interface::CallbackReturn::ERROR;
@@ -61,7 +64,7 @@ namespace hoverboard_hardware_interface
             {
                 RCLCPP_FATAL(
                     rclcpp::get_logger("HoverboardHardwareInterface"),
-                    "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), joint.state_interfaces.size());
+                    "Joint '%s' has %zu state interfaces. 2 expected.", joint.name.c_str(), joint.state_interfaces.size());
                 
                 return hardware_interface::CallbackReturn::ERROR;
             }
@@ -70,7 +73,7 @@ namespace hoverboard_hardware_interface
             {
                 RCLCPP_FATAL(
                     rclcpp::get_logger("HoverboardHardwareInterface"),
-                    "Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(),
+                    "Joint '%s' has '%s' as first state interface. '%s' expected.", joint.name.c_str(),
                     joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
                 
                 return hardware_interface::CallbackReturn::ERROR;
@@ -80,7 +83,7 @@ namespace hoverboard_hardware_interface
             {
                 RCLCPP_FATAL(
                     rclcpp::get_logger("HoverboardHardwareInterface"),
-                    "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(),
+                    "Joint '%s' has '%s' as second state interface. '%s' expected.", joint.name.c_str(),
                     joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY);
                 
                 return hardware_interface::CallbackReturn::ERROR;
@@ -94,11 +97,17 @@ namespace hoverboard_hardware_interface
     {
         std::vector<hardware_interface::StateInterface> state_interfaces;
 
-        state_interfaces.emplace_back(hardware_interface::StateInterface(leftWheel.name, hardware_interface::HW_IF_POSITION, &leftWheel.position));
-        state_interfaces.emplace_back(hardware_interface::StateInterface(leftWheel.name, hardware_interface::HW_IF_VELOCITY, &leftWheel.velocity));
+        state_interfaces.emplace_back(hardware_interface::StateInterface(frontLeftWheel.name, hardware_interface::HW_IF_POSITION, &frontLeftWheel.position));
+        state_interfaces.emplace_back(hardware_interface::StateInterface(frontLeftWheel.name, hardware_interface::HW_IF_VELOCITY, &frontLeftWheel.velocity));
 
-        state_interfaces.emplace_back(hardware_interface::StateInterface(rightWheel.name, hardware_interface::HW_IF_POSITION, &rightWheel.position));
-        state_interfaces.emplace_back(hardware_interface::StateInterface(rightWheel.name, hardware_interface::HW_IF_VELOCITY, &rightWheel.velocity));
+        state_interfaces.emplace_back(hardware_interface::StateInterface(frontRightWheel.name, hardware_interface::HW_IF_POSITION, &frontRightWheel.position));
+        state_interfaces.emplace_back(hardware_interface::StateInterface(frontRightWheel.name, hardware_interface::HW_IF_VELOCITY, &frontRightWheel.velocity));
+
+        state_interfaces.emplace_back(hardware_interface::StateInterface(backLeftWheel.name, hardware_interface::HW_IF_POSITION, &backLeftWheel.position));
+        state_interfaces.emplace_back(hardware_interface::StateInterface(backLeftWheel.name, hardware_interface::HW_IF_VELOCITY, &backLeftWheel.velocity));
+
+        state_interfaces.emplace_back(hardware_interface::StateInterface(backRightWheel.name, hardware_interface::HW_IF_POSITION, &backRightWheel.position));
+        state_interfaces.emplace_back(hardware_interface::StateInterface(backRightWheel.name, hardware_interface::HW_IF_VELOCITY, &backRightWheel.velocity));
 
         return state_interfaces;
     }
@@ -107,8 +116,10 @@ namespace hoverboard_hardware_interface
     {
         std::vector<hardware_interface::CommandInterface> command_interfaces;
 
-        command_interfaces.emplace_back(hardware_interface::CommandInterface(leftWheel.name, hardware_interface::HW_IF_VELOCITY, &leftWheel.command));
-        command_interfaces.emplace_back(hardware_interface::CommandInterface(rightWheel.name, hardware_interface::HW_IF_VELOCITY, &rightWheel.command));
+        command_interfaces.emplace_back(hardware_interface::CommandInterface(frontLeftWheel.name, hardware_interface::HW_IF_VELOCITY, &frontLeftWheel.command));
+        command_interfaces.emplace_back(hardware_interface::CommandInterface(frontRightWheel.name, hardware_interface::HW_IF_VELOCITY, &frontRightWheel.command));
+        command_interfaces.emplace_back(hardware_interface::CommandInterface(backLeftWheel.name, hardware_interface::HW_IF_VELOCITY, &backLeftWheel.command));
+        command_interfaces.emplace_back(hardware_interface::CommandInterface(backRightWheel.name, hardware_interface::HW_IF_VELOCITY, &backRightWheel.command));
 
         return command_interfaces;
     }
@@ -117,12 +128,15 @@ namespace hoverboard_hardware_interface
     {
         RCLCPP_INFO(rclcpp::get_logger("HoverboardHardwareInterface"), "Configuring... please wait a moment...");
 
-        if (!serialPortService.connect(serialPortConfig.device, serialPortConfig.baudRate, serialPortConfig.timeout))
+        if (!connect())
         {
             return hardware_interface::CallbackReturn::ERROR;
         }
 
-        serialPortService.BindMotorWheelFeedbackCallback(
+        frontSerialPortService.BindMotorWheelFeedbackCallback(
+            std::bind(&HoverboardHardwareInterface::motorWheelFeedbackCallback, this, std::placeholders::_1)
+        );
+        backSerialPortService.BindMotorWheelFeedbackCallback(
             std::bind(&HoverboardHardwareInterface::motorWheelFeedbackCallback, this, std::placeholders::_1)
         );
 
@@ -133,7 +147,7 @@ namespace hoverboard_hardware_interface
     {
         RCLCPP_INFO(rclcpp::get_logger("HoverboardHardwareInterface"), "Cleaning up... please wait a moment...");
 
-        if (!serialPortService.disconnect())
+        if (!disconnect())
         {
             return hardware_interface::CallbackReturn::ERROR;
         }
@@ -159,43 +173,87 @@ namespace hoverboard_hardware_interface
 
     hardware_interface::return_type HoverboardHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration & period)
     {
-        serialPortService.read();
+        frontSerialPortService.read();
+        backSerialPortService.read();
 
-        double lastPosition = leftWheel.position;
-        leftWheel.position = leftWheel.calculateEncoderAngle();
-        leftWheel.velocity = (leftWheel.position - lastPosition) / period.seconds();
+        double lastPosition = frontLeftWheel.position;
+        frontLeftWheel.position = frontLeftWheel.calculateEncoderAngle();
+        frontLeftWheel.velocity = (frontLeftWheel.position - lastPosition) / period.seconds();
 
-        lastPosition = rightWheel.position;
-        rightWheel.position = rightWheel.calculateEncoderAngle();
-        rightWheel.velocity = (rightWheel.position - lastPosition) / period.seconds();
+        lastPosition = frontRightWheel.position;
+        frontRightWheel.position = frontRightWheel.calculateEncoderAngle();
+        frontRightWheel.velocity = (frontRightWheel.position - lastPosition) / period.seconds();
+
+        lastPosition = backLeftWheel.position;
+        backLeftWheel.position = backLeftWheel.calculateEncoderAngle();
+        backLeftWheel.velocity = (backLeftWheel.position - lastPosition) / period.seconds();
+
+        lastPosition = backRightWheel.position;
+        backRightWheel.position = backRightWheel.calculateEncoderAngle();
+        backRightWheel.velocity = (backRightWheel.position - lastPosition) / period.seconds();
 
         return hardware_interface::return_type::OK;
     }
 
     hardware_interface::return_type HoverboardHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &)
     {
-        MotorWheelDriveControl motorWheelDriveControl;
+        MotorWheelDriveControl frontMotorWheelDriveControl;
+        MotorWheelDriveControl backMotorWheelDriveControl;
 
-        const double speed = ((leftWheel.command / 0.10472) + (rightWheel.command / 0.10472)) / 2.0;
-        const double steer = ((leftWheel.command / 0.10472) - speed) * 2.0;
+        const double frontSpeed = ((frontLeftWheel.command / 0.10472) + (frontRightWheel.command / 0.10472)) / 2.0;
+        const double frontSteer = ((frontLeftWheel.command / 0.10472) - frontSpeed) * 2.0;
+
+        const double backSpeed = ((backLeftWheel.command / 0.10472) + (backRightWheel.command / 0.10472)) / 2.0;
+        const double backSteer = ((backLeftWheel.command / 0.10472) - backSpeed) * 2.0;
 
         // TODO: radius should be read from the urdf file, check calculations
-        motorWheelDriveControl.speed = (int16_t) (speed);
-        motorWheelDriveControl.steer = (int16_t) (steer);
-        motorWheelDriveControl.checksum = (uint16_t)(motorWheelDriveControl.head ^ motorWheelDriveControl.steer ^ motorWheelDriveControl.speed);
+        frontMotorWheelDriveControl.speed = static_cast<int16_t>(frontSpeed);
+        frontMotorWheelDriveControl.steer = static_cast<int16_t>(frontSteer);
+        frontMotorWheelDriveControl.checksum = static_cast<uint16_t>(frontMotorWheelDriveControl.head ^ frontMotorWheelDriveControl.steer ^ frontMotorWheelDriveControl.speed);
+
+        backMotorWheelDriveControl.speed = static_cast<int16_t>(backSpeed);
+        backMotorWheelDriveControl.steer = static_cast<int16_t>(backSteer);
+        backMotorWheelDriveControl.checksum = static_cast<uint16_t>(backMotorWheelDriveControl.head ^ backMotorWheelDriveControl.steer ^ backMotorWheelDriveControl.speed);
+
+        RCLCPP_INFO(rclcpp::get_logger("SerialPortService"), "Front: %i %i", frontMotorWheelDriveControl.speed, frontMotorWheelDriveControl.steer);
+        RCLCPP_INFO(rclcpp::get_logger("SerialPortService"), "Back: %i %i", backMotorWheelDriveControl.speed, backMotorWheelDriveControl.steer);
+        
+        RCLCPP_INFO(rclcpp::get_logger("SerialPortService"), "Sending front motor commands: speed=%i, steer=%i", frontMotorWheelDriveControl.speed, frontMotorWheelDriveControl.steer);
+        RCLCPP_INFO(rclcpp::get_logger("SerialPortService"), "Sending back motor commands: speed=%i, steer=%i", backMotorWheelDriveControl.speed, backMotorWheelDriveControl.steer);
+        
+        RCLCPP_INFO(rclcpp::get_logger("HoverboardHardwareInterface"), "Writing commands to motors...");
+        
+        
+        frontSerialPortService.write(reinterpret_cast<const char*>(&frontMotorWheelDriveControl), sizeof(MotorWheelDriveControl));
+        backSerialPortService.write(reinterpret_cast<const char*>(&backMotorWheelDriveControl), sizeof(MotorWheelDriveControl));
+
+        return hardware_interface::return_type::OK;
+    }
 
-         RCLCPP_INFO(rclcpp::get_logger("SerialPortService"), "%i %i", motorWheelDriveControl.speed, motorWheelDriveControl.steer);
+    void HoverboardHardwareInterface::motorWheelFeedbackCallback(MotorWheelFeedback motorWheelFeedback)
+    {
+        frontLeftWheel.updateEncoderTicks(motorWheelFeedback.leftMotorEncoderCumulativeCount);
+        frontRightWheel.updateEncoderTicks(motorWheelFeedback.rightMotorEncoderCumulativeCount);
+        backLeftWheel.updateEncoderTicks(motorWheelFeedback.leftMotorEncoderCumulativeCount);
+        backRightWheel.updateEncoderTicks(motorWheelFeedback.rightMotorEncoderCumulativeCount);
+    }
 
-        serialPortService.write((const char*) &motorWheelDriveControl, sizeof(MotorWheelDriveControl));
+    bool HoverboardHardwareInterface::connect()
+    {
+        bool frontConnected = frontSerialPortService.connect(serialPortConfig.frontDevice, serialPortConfig.baudRate, serialPortConfig.timeout);
+        bool backConnected = backSerialPortService.connect(serialPortConfig.backDevice, serialPortConfig.baudRate, serialPortConfig.timeout);
 
-        return hardware_interface::return_type::OK;
+        return frontConnected && backConnected;
     }
 
-    void HoverboardHardwareInterface::motorWheelFeedbackCallback(MotorWheelFeedback motorWheelFeedback) 
+    bool HoverboardHardwareInterface::disconnect()
     {
-        leftWheel.updateEncoderTicks(motorWheelFeedback.leftMotorEncoderCumulativeCount);
-        rightWheel.updateEncoderTicks(motorWheelFeedback.rightMotorEncoderCumulativeCount);
+        bool frontDisconnected = frontSerialPortService.disconnect();
+        bool backDisconnected = backSerialPortService.disconnect();
+
+        return frontDisconnected && backDisconnected;
     }
 }
 
 PLUGINLIB_EXPORT_CLASS(hoverboard_hardware_interface::HoverboardHardwareInterface, hardware_interface::SystemInterface)
+
diff --git a/hoverboard_ros2_control/README.md b/hoverboard_ros2_control/README.md
new file mode 100644
index 0000000..72ac932
--- /dev/null
+++ b/hoverboard_ros2_control/README.md
@@ -0,0 +1,48 @@
+# ROS2 Control Hoverboard Hardware Interface
+
+ROS2 Control hardware interface for Hoverboard motors. Implementation in C++ with Boost libraries. All steering and feedback messages are transmitted by serial port communication.
+
+**Work is still in progress, we will be adding new features.**
+
+### Requirements
+
+Hoverboard driver firmware: [ https://github.com/hoverboard-robotics/hoverboard-firmware-hack-FOC]( https://github.com/hoverboard-robotics/hoverboard-firmware-hack-FOC)
+
+All ROS2 and system dependencies are installable by the rosdep command-line tool. Just run the following command:
+
+```sh
+rosdep install --from-paths src -y --ignore-src
+```
+
+You can learn more about the rosdep command line tool here: [https://docs.ros.org/en/humble/Tutorials/Intermediate/Rosdep.html](https://docs.ros.org/en/humble/Tutorials/Intermediate/Rosdep.html)
+
+### How to run it?
+
+The simplest way to run the whole package is to use the demo launch file. The launch file can be an inspiration for your custom usage of the package.
+
+To use the launch file, just run the following command:
+
+```sh
+ros2 launch hoverboard_demo_bringup hoverboard.launch.py
+```
+
+### Inspirations
+
+* [https://github.com/Factor-Robotics/odrive_ros2_control](https://github.com/Factor-Robotics/odrive_ros2_control/tree/humble-fw-v0.5.3)
+* [https://github.com/joshnewans/diffdrive_arduino](https://github.com/joshnewans/diffdrive_arduino/tree/humble)
+
+**Thank you for your support and inspiration!**
+
+### Tests
+
+The code works well with USB to TTL adapter with a 3.3V voltage level and with a UART1 port on the Jetson Xavier NX Development Kit. The code was tested with ROS2 Humble distribution with a standalone and containerized (Docker) setup.
+
+### Cooperation and contact
+
+If you want to add or change something, just fork this repository and push changes by the pull request. If you need something, but you can't do it yourself, please write an issue. We will do our best to help.
+
+### Team
+
+* [Bart Van der Haagen](https://www.linkedin.com/in/bart-van-der-haagen-b91b10bb/)
+* [Robert Gruberski](https://www.linkedin.com/in/rgruberski/)
+
diff --git a/hoverboard_ros2_control/hoverboard_demo_bringup/CMakeLists.txt b/hoverboard_ros2_control/hoverboard_demo_bringup/CMakeLists.txt
new file mode 100644
index 0000000..30e2cda
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_demo_bringup/CMakeLists.txt
@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.8)
+project(hoverboard_demo_bringup)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE config launch)
+
diff --git a/hoverboard_ros2_control/hoverboard_demo_bringup/config/hoverboard_controllers.yaml b/hoverboard_ros2_control/hoverboard_demo_bringup/config/hoverboard_controllers.yaml
new file mode 100644
index 0000000..a84d8be
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_demo_bringup/config/hoverboard_controllers.yaml
@@ -0,0 +1,57 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 50  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    hoverboard_base_controller:
+      type: diff_drive_controller/DiffDriveController
+
+hoverboard_base_controller:
+  ros__parameters:
+    left_wheel_names: ["driving_wheel_front_left_joint", "driving_wheel_back_left_joint"]
+    right_wheel_names: ["driving_wheel_front_right_joint", "driving_wheel_back_right_joint"]
+
+    wheel_separation: 0.44
+    #wheels_per_side: 1  # actually 2, but both are controlled by 1 sign
+    wheel_radius: 0.08255 # 0.085
+
+    wheel_separation_multiplier: 1.0
+    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
+    pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+
+    open_loop: false
+    enable_odom_tf: true
+
+    cmd_vel_timeout: 0.5
+    #publish_limited_velocity: true
+    use_stamped_vel: false
+    #velocity_rolling_window_size: 10
+
+    # Velocity and acceleration limits
+    # Whenever a min_* is unspecified, default to -max_*
+    linear.x.has_velocity_limits: true
+    linear.x.has_acceleration_limits: true
+    linear.x.has_jerk_limits: false
+    linear.x.max_velocity: 1.0
+    linear.x.min_velocity: -1.0
+    linear.x.max_acceleration: 1.0
+    linear.x.max_jerk: 0.0
+    linear.x.min_jerk: 0.0
+
+    angular.z.has_velocity_limits: true
+    angular.z.has_acceleration_limits: true
+    angular.z.has_jerk_limits: false
+    angular.z.max_velocity: 1.0
+    angular.z.min_velocity: -1.0
+    angular.z.max_acceleration: 1.0
+    angular.z.min_acceleration: -1.0
+    angular.z.max_jerk: 0.0
+    angular.z.min_jerk: 0.0
diff --git a/hoverboard_ros2_control/hoverboard_demo_bringup/config/ps4.config.yaml b/hoverboard_ros2_control/hoverboard_demo_bringup/config/ps4.config.yaml
new file mode 100644
index 0000000..135d5dc
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_demo_bringup/config/ps4.config.yaml
@@ -0,0 +1,18 @@
+teleop_twist_joy_node:
+  ros__parameters:
+    axis_linear:  # Left thumb stick vertical
+      x: 1
+    scale_linear:
+      x: 0.8
+    scale_linear_turbo:
+      x: 1.5
+
+    axis_angular:  # Left thumb stick horizontal
+      yaw: 0
+    scale_angular:
+      yaw: 0.7
+
+    require_enable_button: false
+
+    # enable_button: 2  # Left trigger button
+    enable_turbo_button: 5  # Right trigger button
diff --git a/hoverboard_ros2_control/hoverboard_demo_bringup/launch/hoverboard.launch.py b/hoverboard_ros2_control/hoverboard_demo_bringup/launch/hoverboard.launch.py
new file mode 100644
index 0000000..d0b422a
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_demo_bringup/launch/hoverboard.launch.py
@@ -0,0 +1,133 @@
+# Copyright 2023 Robert Gruberski (Viola Robotics Sp. z o.o. Poland)
+#
+# 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_ros.actions import Node
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("hoverboard_demo_description"), "urdf", "hoverboard_description.xacro"
+                ]
+            ),
+        ]
+    )
+
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("hoverboard_demo_bringup"), "config", "hoverboard_controllers.yaml",
+        ]
+    )
+
+    rviz_config_file = PathJoinSubstitution(
+        [
+            FindPackageShare("hoverboard_demo_description"), "rviz", "hoverboard.rviz"
+        ]
+    )
+
+    teleop_twist_joy_config_file = PathJoinSubstitution(
+        [
+            FindPackageShare("hoverboard_demo_bringup"), "config", "ps4.config.yaml",
+        ]
+    )
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_description, robot_controllers],
+        output="both",
+    )
+
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
+    )
+
+    robot_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["hoverboard_base_controller", "-c", "/controller_manager"]
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        arguments=["-d", rviz_config_file],
+    )
+
+    joy_node = Node(
+        package='joy', executable='joy_node', name='joy_node',
+        parameters=[{
+            'deadzone': 0.05,
+            'autorepeat_rate': 30.0
+        }],
+    )
+
+    teleop_twist_joy_node = Node(
+        package='teleop_twist_joy',
+        executable='teleop_node',
+        name='teleop_twist_joy_node',
+        parameters=[teleop_twist_joy_config_file],
+        remappings=[
+            ('/cmd_vel', '/hoverboard_base_controller/cmd_vel_unstamped'),
+        ]
+    )
+
+
+       
+
+    lidar_node = Node(
+        name='rplidar_composition',
+        package='rplidar_ros',
+        executable='rplidar_composition',
+        output='screen',
+        parameters=[{
+            'serial_port': '/dev/ttyUSB2',
+            'serial_baudrate': 115200,  # A1 / A2
+            # 'serial_baudrate': 256000, # A3
+            'frame_id': 'lidar',
+            'inverted': False,
+            'angle_compensate': True,}]
+    )
+
+    return LaunchDescription([
+
+    
+        lidar_node,
+        control_node,
+        robot_state_pub_node,
+        joint_state_broadcaster_spawner,
+        robot_controller_spawner,
+        rviz_node,
+        # joy_node,
+        # teleop_twist_joy_node,
+    ])
diff --git a/hoverboard_ros2_control/hoverboard_demo_bringup/package.xml b/hoverboard_ros2_control/hoverboard_demo_bringup/package.xml
new file mode 100644
index 0000000..13d4f17
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_demo_bringup/package.xml
@@ -0,0 +1,27 @@
+<?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>hoverboard_demo_bringup</name>
+  <version>0.1.0</version>
+  <description>Launch files and configurations for Hoverboard Motors</description>
+  <maintainer email="rgruberski@gmail.com">Robert Gruberski</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <exec_depend>hoverboard_demo_description</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>ros2_controllers</exec_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>joy</exec_depend>
+  <exec_depend>teleop_twist_joy</exec_depend>
+
+  <exec_depend>xacro</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/CMakeLists.txt b/hoverboard_ros2_control/hoverboard_demo_description/CMakeLists.txt
new file mode 100644
index 0000000..c770631
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_demo_description/CMakeLists.txt
@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(hoverboard_demo_description)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE urdf rviz meshes)
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/base_link.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/base_link.stl
new file mode 100644
index 0000000..9eb1ba9
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/base_link.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/front_plate.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/front_plate.stl
new file mode 100644
index 0000000..40afd9a
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/front_plate.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/imu.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/imu.stl
new file mode 100644
index 0000000..925e0e8
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/imu.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/jetson.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/jetson.stl
new file mode 100644
index 0000000..32e65a8
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/jetson.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/left_back_wheel.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/left_back_wheel.stl
new file mode 100644
index 0000000..afa272a
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/left_back_wheel.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/left_front_wheel.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/left_front_wheel.stl
new file mode 100644
index 0000000..f8171e5
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/left_front_wheel.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/left_plate.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/left_plate.stl
new file mode 100644
index 0000000..fd7e1fe
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/left_plate.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/lidar.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/lidar.stl
new file mode 100644
index 0000000..66e5eec
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/lidar.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/logo1.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/logo1.stl
new file mode 100644
index 0000000..fd2829e
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/logo1.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/logo2.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/logo2.stl
new file mode 100644
index 0000000..8d67166
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/logo2.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/mount_link.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/mount_link.stl
new file mode 100644
index 0000000..83c2909
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/mount_link.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/plate.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/plate.stl
new file mode 100644
index 0000000..79dc969
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/plate.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/rear_plate.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/rear_plate.stl
new file mode 100644
index 0000000..d1ede74
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/rear_plate.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/right_back_wheel.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/right_back_wheel.stl
new file mode 100644
index 0000000..5e0ba84
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/right_back_wheel.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/right_front_wheel.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/right_front_wheel.stl
new file mode 100644
index 0000000..034ebc2
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/right_front_wheel.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/right_plate.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/right_plate.stl
new file mode 100644
index 0000000..691cc3b
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/right_plate.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/top_cover.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/top_cover.stl
new file mode 100644
index 0000000..a2a3a3f
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/top_cover.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/meshes/zed_mini.stl b/hoverboard_ros2_control/hoverboard_demo_description/meshes/zed_mini.stl
new file mode 100644
index 0000000..1379d4d
Binary files /dev/null and b/hoverboard_ros2_control/hoverboard_demo_description/meshes/zed_mini.stl differ
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/package.xml b/hoverboard_ros2_control/hoverboard_demo_description/package.xml
new file mode 100644
index 0000000..74aa01f
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_demo_description/package.xml
@@ -0,0 +1,20 @@
+<?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>hoverboard_demo_description</name>
+  <version>0.1.0</version>
+  <description>Description (urdf) files and RVIZ configurations for Hoverboard Motors</description>
+  <maintainer email="rgruberski@gmail.com">Robert Gruberski</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <exec_depend>rviz2</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/rviz/hoverboard.rviz b/hoverboard_ros2_control/hoverboard_demo_description/rviz/hoverboard.rviz
new file mode 100644
index 0000000..91a3c92
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_demo_description/rviz/hoverboard.rviz
@@ -0,0 +1,237 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      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
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base:
+          Value: true
+        base_link:
+          Value: true
+        caster_wheel_front:
+          Value: true
+        caster_wheel_holder:
+          Value: true
+        chassis:
+          Value: true
+        driving_wheel_left:
+          Value: true
+        driving_wheel_right:
+          Value: true
+        odom:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        odom:
+          base_link:
+            base:
+              {}
+            caster_wheel_front:
+              {}
+            caster_wheel_holder:
+              {}
+            chassis:
+              {}
+            driving_wheel_left:
+              {}
+            driving_wheel_right:
+              {}
+      Update Interval: 0
+      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:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        caster_wheel_front:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_wheel_holder:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        chassis:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        driving_wheel_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        driving_wheel_right:
+          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: 4.256966590881348
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.23792937397956848
+        Y: 0.30662044882774353
+        Z: -0.048571959137916565
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.785398006439209
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c90000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1850
+  X: 1990
+  Y: 27
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/urdf/hoverboard_description.xacro b/hoverboard_ros2_control/hoverboard_demo_description/urdf/hoverboard_description.xacro
new file mode 100644
index 0000000..4283aa4
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_demo_description/urdf/hoverboard_description.xacro
@@ -0,0 +1,250 @@
+<?xml version="1.0"?>
+<robot name="hoverboard_robot" xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <xacro:include filename="materials.xacro" />
+    <xacro:include filename="macros.xacro" />
+    <xacro:include filename="wheels.xacro" />
+    <xacro:include filename="ros2_control.xacro" />
+
+    <xacro:property name="plate_radius" value="0.57" />
+    <xacro:property name="plate_height" value="0.01" />
+
+    <xacro:property name="chassis_weight" value="1.0" />
+
+    <xacro:property name="driving_wheel_diameter" value="0.165" />
+    <xacro:property name="driving_wheel_thickness" value="0.045" />
+    <xacro:property name="driving_wheel_weight" value="2.2" />
+    <xacro:property name="driving_wheel_base_gap" value="0.01" />
+
+    <xacro:property name="caster_wheel_diameter" value="0.09" />
+    <xacro:property name="caster_wheel_weight" value="0.1" />
+
+    <link name="base_link">
+    </link>
+
+    <link name="plate">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh filename="package://hoverboard_demo_description/meshes/base_link.stl" scale="0.001 0.001 0.001"/>
+            </geometry>
+            <material name="gray"/>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh filename="package://hoverboard_demo_description/meshes/base_link.stl" scale="0.001 0.001 0.001"/>
+            </geometry>
+        </collision>
+        <xacro:cylinder_inertia m="0.5" r="${plate_radius}" h="${plate_height}" roll="${pi/2}"/>
+    </link>
+    
+   	 <link name="lidar">
+ 	 <inertial>
+ 	   <origin xyz="-0.008294099570319567 -3.8749192144224396e-17 0.05020114222279101" rpy="0 0 0"/>
+ 	   <mass value="1.1515616542314224"/>
+ 	   <inertia ixx="0.000489" iyy="0.000861" izz="0.000987" ixy="-0.0" iyz="-0.0" ixz="-0.000148"/>
+	  </inertial>
+	  <visual>
+	    <origin xyz="-0.0 -0.0 -0.141" rpy="0 0 0"/>
+	    <geometry>
+ 	     <mesh filename="file://$(find hoverboard_demo_description)/meshes/lidar.stl" scale="0.001 0.001 0.001"/>
+	    </geometry>
+	    <material name="black"/>
+ 	 </visual>
+	  <collision>
+  	  <origin xyz="-0.0 -0.0 -0.141" rpy="0 0 0"/>
+ 	   <geometry>
+  	    <mesh filename="file://$(find hoverboard_demo_description)/meshes/lidar.stl" scale="0.001 0.001 0.001"/>
+  	  </geometry>
+ 	 </collision>
+	</link>
+	
+	<link name="imu">
+  	<inertial>
+  	  <origin xyz="-0.01898812297806371 9.133494040341156e-05 0.000862164665477741" rpy="0 0 0"/>
+  	  <mass value="0.004997129426507204"/>
+  	  <inertia ixx="0.0" iyy="0.0" izz="0.0" ixy="0.0" iyz="-0.0" ixz="-0.0"/>
+	  </inertial>
+	  <visual>
+ 	   <origin xyz="0.164479 -0.0 -0.108" rpy="0 0 0"/>
+ 	   <geometry>
+	      <mesh filename="file://$(find hoverboard_demo_description)/meshes/imu.stl" scale="0.001 0.001 0.001"/>
+	    </geometry>
+ 	   <material name="black"/>
+ 	 </visual>
+ 	 <collision>
+  	  <origin xyz="0.164479 -0.0 -0.108" rpy="0 0 0"/>
+  	  <geometry>
+  	    <mesh filename="file://$(find hoverboard_demo_description)/meshes/imu.stl" scale="0.001 0.001 0.001"/>
+  	  </geometry>
+ 	 </collision>
+	</link>
+	
+	<link name="front_plate">
+	  <inertial>
+ 	   <origin xyz="-0.00500000000000006 -2.416750931496303e-16 0.05201635561346364" rpy="0 0 0"/>
+	    <mass value="1.8028979735720785"/>
+	    <inertia ixx="0.02436" iyy="0.001727" izz="0.022643" ixy="0.0" iyz="0.0" ixz="-0.0"/>
+	  </inertial>
+	  <visual>
+ 	   <origin xyz="-0.32 -0.0 -0.071" rpy="0 0 0"/>
+ 	   <geometry>
+	      <mesh filename="file://$(find hoverboard_demo_description)/meshes/front_plate.stl" scale="0.001 0.001 0.001"/>
+	    </geometry>
+ 	   <material name="blue"/>
+ 	 </visual>
+ 	 <collision>
+ 	   <origin xyz="-0.32 -0.0 -0.071" rpy="0 0 0"/>
+ 	   <geometry>
+ 	     <mesh filename="file://$(find hoverboard_demo_description)/meshes/front_plate.stl" scale="0.001 0.001 0.001"/>
+ 	   </geometry>
+ 	 </collision>
+	</link>
+
+	<link name="rear_plate">
+	  <inertial>
+  	  <origin xyz="0.003999999999999948 -2.6213617996365013e-16 0.052162485409430734" rpy="0 0 0"/>
+  	  <mass value="1.9065179735720788"/>
+ 	   <inertia ixx="0.025647" iyy="0.001735" izz="0.023924" ixy="0.0" iyz="0.0" ixz="-0.0"/>
+  	</inertial>
+  	<visual>
+ 	   <origin xyz="0.32 -0.0 -0.071" rpy="0 0 0"/>
+  	  <geometry>
+  	    <mesh filename="file://$(find hoverboard_demo_description)/meshes/rear_plate.stl" scale="0.001 0.001 0.001"/>
+  	  </geometry>
+ 	   <material name="blue"/>
+ 	 </visual>
+ 	 <collision>
+ 	   <origin xyz="0.32 -0.0 -0.071" rpy="0 0 0"/>
+ 	   <geometry>
+ 	     <mesh filename="file://$(find hoverboard_demo_description)/meshes/rear_plate.stl" scale="0.001 0.001 0.001"/>
+ 	   </geometry>
+ 	 </collision>
+	</link>
+
+	<link name="right_plate">
+ 	 <inertial>
+ 	   <origin xyz="-0.0010632594372099004 0.002000000000000085 0.06859611162847648" rpy="0 0 0"/>
+ 	   <mass value="2.2154231115200234"/>
+ 	   <inertia ixx="0.002726" iyy="0.075573" izz="0.07286" ixy="0.0" iyz="0.0" ixz="-2.2e-05"/>
+ 	 </inertial>
+ 	 <visual>
+ 	   <origin xyz="-0.0 0.24 -0.065" rpy="0 0 0"/>
+ 	   <geometry>
+  	    <mesh filename="file://$(find hoverboard_demo_description)/meshes/right_plate.stl" scale="0.001 0.001 0.001"/>
+ 	   </geometry>
+  	  <material name="blue"/>
+ 	 </visual>
+  	<collision>
+  	  <origin xyz="-0.0 0.24 -0.065" rpy="0 0 0"/>
+  	  <geometry>
+  	    <mesh filename="file://$(find hoverboard_demo_description)/meshes/right_plate.stl" scale="0.001 0.001 0.001"/>
+  	  </geometry>
+ 	 </collision>
+	</link>
+
+	<link name="left_plate">
+ 	 <inertial>
+ 	   <origin xyz="-0.001063259437208634 -0.0019999999999998908 0.06859611162847654" rpy="0 0 0"/>
+ 	   <mass value="2.2154231115200127"/>
+ 	   <inertia ixx="0.002726" iyy="0.075573" izz="0.07286" ixy="-0.0" iyz="-0.0" ixz="-2.2e-05"/>
+	  </inertial>
+	  <visual>
+	    <origin xyz="-0.0 -0.24 -0.065" rpy="0 0 0"/>
+	    <geometry>
+ 	     <mesh filename="file://$(find hoverboard_demo_description)/meshes/left_plate.stl" scale="0.001 0.001 0.001"/>
+ 	   </geometry>
+	    <material name="blue"/>
+ 	 </visual>
+ 	 <collision>
+ 	   <origin xyz="-0.0 -0.24 -0.065" rpy="0 0 0"/>
+  	  <geometry>
+ 	     <mesh filename="file://$(find hoverboard_demo_description)/meshes/left_plate.stl" scale="0.001 0.001 0.001"/>
+	    </geometry>
+	  </collision>
+	</link>
+
+	<link name="top_cover">
+	  <inertial>
+ 	   <origin xyz="-0.16950000288337444 0.17099999714191824 0.003000000000000058" rpy="0 0 0"/>
+ 	   <mass value="5.769750000000013"/>
+ 	   <inertia ixx="0.058917" iyy="0.058917" izz="0.117799" ixy="0.0" iyz="-0.0" ixz="-0.0"/>
+	  </inertial>
+	  <visual>
+ 	   <origin xyz="-0.1695 0.171 -0.2165" rpy="0 0 0"/>
+ 	   <geometry>
+ 	     <mesh filename="file://$(find hoverboard_demo_description)/meshes/top_cover.stl" scale="0.001 0.001 0.001"/>
+ 	   </geometry>
+ 	   <material name="blue"/>
+	  </visual>
+	  <collision>
+	    <origin xyz="-0.1695 0.171 -0.2165" rpy="0 0 0"/>
+	    <geometry>
+ 	     <mesh filename="file://$(find hoverboard_demo_description)/meshes/top_cover.stl" scale="0.001 0.001 0.001"/>
+ 	   </geometry>
+ 	 </collision>
+	</link>
+
+
+    <joint name="base_link_plate_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 0" />
+        <parent link="base_link" />
+        <child link="plate" />
+    </joint>
+    
+    <joint name="lidar_joint" type="fixed">
+  <origin xyz="0.0 0.0 0.141" rpy="0 0 0"/>
+  <parent link="base_link"/>
+  <child link="lidar"/>
+</joint>
+
+<joint name="imu_joint" type="fixed">
+  <origin xyz="-0.164479 0.0 0.108" rpy="0 0 0"/>
+  <parent link="base_link"/>
+  <child link="imu"/>
+</joint>
+
+<joint name="front_plate_joint" type="fixed">
+  <origin xyz="0.32 0.0 0.071" rpy="0 0 0"/>
+  <parent link="base_link"/>
+  <child link="front_plate"/>
+</joint>
+
+<joint name="rear_plate_joint" type="fixed">
+  <origin xyz="-0.32 0.0 0.071" rpy="0 0 0"/>
+  <parent link="base_link"/>
+  <child link="rear_plate"/>
+</joint>
+
+<joint name="right_plate_joint" type="fixed">
+  <origin xyz="0.0 -0.24 0.065" rpy="0 0 0"/>
+  <parent link="base_link"/>
+  <child link="right_plate"/>
+</joint>
+
+<joint name="left_plate_joint" type="fixed">
+  <origin xyz="0.0 0.24 0.065" rpy="0 0 0"/>
+  <parent link="base_link"/>
+  <child link="left_plate"/>
+</joint>
+
+<joint name="top_cover_joint" type="fixed">
+  <origin xyz="0.1695 -0.171 0.2165" rpy="0 0 0"/>
+  <parent link="base_link"/>
+  <child link="top_cover"/>
+</joint>
+
+
+    <xacro:driving_wheel suffix="front_left" reflect="1" position="0.19"/>
+    <xacro:driving_wheel suffix="front_right" reflect="-1" position="0.19"/>
+
+    <xacro:driving_wheel suffix="back_left" reflect="1" position="-0.19"/>
+    <xacro:driving_wheel suffix="back_right" reflect="-1" position="-0.19"/>
+
+    <!-- <xacro:caster_wheel suffix="front" reflect="1" />
+    <xacro:caster_wheel suffix="back" reflect="-1" /> -->
+
+    <xacro:hoverboard_ros2_control name="hoverboard" />
+
+</robot>
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/urdf/macros.xacro b/hoverboard_ros2_control/hoverboard_demo_description/urdf/macros.xacro
new file mode 100644
index 0000000..837e6e4
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_demo_description/urdf/macros.xacro
@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <xacro:macro name="default_inertial" params="mass">
+        <inertial>
+            <mass value="${mass}" />
+            <inertia ixx="1e-3" ixy="0.0" ixz="0.0" iyy="1e-3" iyz="0.0" izz="1e-3"/>
+        </inertial>
+    </xacro:macro>
+
+    <!-- Define some commonly used intertial properties  -->
+    <xacro:macro name="box_inertia" params="m w h d">
+        <inertial>
+            <origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>
+            <mass value="${m}"/>
+            <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>
+        </inertial>
+    </xacro:macro>
+
+    <xacro:macro name="cylinder_inertia" params="m r h roll">
+        <inertial>
+            <origin xyz="0 0 0" rpy="${roll} 0 0" />
+            <mass value="${m}"/>
+            <inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy = "0" ixz = "0" iyy="${(m/12) * (3*r*r + h*h)}" iyz = "0" izz="${(m/2) * (r*r)}"/>
+        </inertial>
+    </xacro:macro>
+
+    <xacro:macro name="sphere_inertia" params="m r">
+        <inertial>
+            <mass value="${m}"/>
+            <inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}"/>
+        </inertial>
+    </xacro:macro>
+
+</robot>
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/urdf/materials.xacro b/hoverboard_ros2_control/hoverboard_demo_description/urdf/materials.xacro
new file mode 100644
index 0000000..75a51ca
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_demo_description/urdf/materials.xacro
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <material name="silver">
+        <color rgba="0.60 0.60 0.60 1"/>
+    </material>
+
+    <material name="gray">
+        <color rgba="0.40 0.40 0.40 1"/>
+    </material>
+
+    <material name="black">
+        <color rgba="0 0 0 1"/>
+    </material>
+
+    <material name="blue">
+        <color rgba="0 0 1 1"/>
+    </material>
+
+</robot>
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/urdf/ros2_control.xacro b/hoverboard_ros2_control/hoverboard_demo_description/urdf/ros2_control.xacro
new file mode 100644
index 0000000..a05c5ad
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_demo_description/urdf/ros2_control.xacro
@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="hoverboard_ros2_control" params="name">
+
+    <ros2_control name="${name}_front" type="system">
+      <hardware>
+        <plugin>hoverboard_hardware_interface/HoverboardHardwareInterface</plugin>
+        <param name="left_wheel_joint_name">driving_wheel_front_left_joint</param>
+        <param name="right_wheel_joint_name">driving_wheel_front_right_joint</param>
+        <param name="loop_rate">50</param>
+        <param name="device">/dev/ttyUSB0</param>
+        <param name="baud_rate">115200</param>
+        <param name="timeout">1000</param>
+        <param name="encoder_ticks_per_revolution">90</param>
+      </hardware>
+      <joint name="driving_wheel_front_left_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
+      <joint name="driving_wheel_front_right_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
+    </ros2_control>
+
+    <ros2_control name="${name}_back" type="system">
+      <hardware>
+        <plugin>hoverboard_hardware_interface/HoverboardHardwareInterface</plugin>
+        <param name="left_wheel_joint_name">driving_wheel_back_left_joint</param>
+        <param name="right_wheel_joint_name">driving_wheel_back_right_joint</param>
+        <param name="loop_rate">50</param>
+        <param name="device">/dev/ttyUSB1</param>
+        <param name="baud_rate">115200</param>
+        <param name="timeout">1000</param>
+        <param name="encoder_ticks_per_revolution">90</param>
+      </hardware>
+      <joint name="driving_wheel_back_left_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
+      <joint name="driving_wheel_back_right_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
+    </ros2_control>
+
+  </xacro:macro>
+
+</robot>
diff --git a/hoverboard_ros2_control/hoverboard_demo_description/urdf/wheels.xacro b/hoverboard_ros2_control/hoverboard_demo_description/urdf/wheels.xacro
new file mode 100644
index 0000000..3631f95
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_demo_description/urdf/wheels.xacro
@@ -0,0 +1,78 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <!-- TODO: The model is made in a very simple form, it would be nice to make it a bit better. Is someone willing? -->
+    <xacro:macro name="driving_wheel" params="suffix reflect position">
+        <link name="driving_wheel_${suffix}">
+            <visual>
+                <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
+                <geometry>
+                    <cylinder radius="${driving_wheel_diameter/2}" length="${driving_wheel_thickness}"/>
+                </geometry>
+                <material name="black"/>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
+                <contact_coefficients mu="100.0" kp="0.0" kd="0.0" />
+                <geometry>
+                    <cylinder radius="${driving_wheel_diameter/2}" length="${driving_wheel_thickness}"/>
+                </geometry>
+            </collision>
+            <xacro:cylinder_inertia m="${driving_wheel_weight}" r="${driving_wheel_diameter/2}" h="${driving_wheel_thickness}" roll="${pi/2}"/>
+        </link>
+
+        <joint name="driving_wheel_${suffix}_joint" type="continuous">
+            <axis xyz="0 1 0" rpy="0 0 0" />
+            <parent link="base_link"/>
+            <child link="driving_wheel_${suffix}"/>
+            <origin xyz="${position} ${(plate_radius+driving_wheel_thickness+driving_wheel_base_gap-0.195)/2*reflect} 0.09" rpy="0 0 0"/>
+        </joint>
+
+        <gazebo reference="driving_wheel_${suffix}">
+            <mu1 value="0.50"/>
+            <mu2 value="3.0"/>
+            <kp value="10000000.0" />
+            <kd value="100.0" />
+            <fdir1 value="0 0 1"/>
+            <material>Gazebo/Black</material>
+        </gazebo>
+
+    </xacro:macro>
+
+    <xacro:macro name="caster_wheel" params="suffix reflect">
+        <link name="caster_wheel_${suffix}">
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <sphere radius="${caster_wheel_diameter/2}" />
+                </geometry>
+                <material name="black"/>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <contact_coefficients mu="0.0" kp="0.0" kd="0.0" />
+                <geometry>
+                    <sphere radius="${caster_wheel_diameter/2}" />
+                </geometry>
+            </collision>
+            <xacro:sphere_inertia m="${caster_wheel_weight}" r="${caster_wheel_diameter/2}"/>
+        </link>
+
+        <joint name="caster_wheel_${suffix}_joint" type="fixed">
+            <parent link="base_link"/>
+            <child link="caster_wheel_${suffix}"/>
+            <origin xyz="${0.12*reflect} 0 0" rpy="0 0 0"/>
+        </joint>
+
+        <gazebo reference="caster_wheel_${suffix}">
+            <mu1 value="0.0"/>
+            <mu2 value="0.0"/>
+            <kp value="10000000.0" />
+            <kd value="100.0" />
+            <fdir1 value="0 0 1"/>
+            <material>Gazebo/Red</material>
+        </gazebo>
+
+    </xacro:macro>
+
+</robot>
diff --git a/hoverboard_ros2_control/hoverboard_hardware_interface/CMakeLists.txt b/hoverboard_ros2_control/hoverboard_hardware_interface/CMakeLists.txt
new file mode 100644
index 0000000..29e4064
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_hardware_interface/CMakeLists.txt
@@ -0,0 +1,47 @@
+cmake_minimum_required(VERSION 3.8)
+project(hoverboard_hardware_interface)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system) # thread regex
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(
+  serial_port_service SHARED
+  src/serial_port_service.cpp
+)
+
+ament_auto_add_library(
+  ${PROJECT_NAME} SHARED
+  src/hoverboard_hardware_interface.cpp
+)
+
+target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES})
+
+pluginlib_export_plugin_description_file(hardware_interface hoverboard_hardware_interface.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()
diff --git a/hoverboard_ros2_control/hoverboard_hardware_interface/hoverboard_hardware_interface.xml b/hoverboard_ros2_control/hoverboard_hardware_interface/hoverboard_hardware_interface.xml
new file mode 100644
index 0000000..cafd400
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_hardware_interface/hoverboard_hardware_interface.xml
@@ -0,0 +1,9 @@
+<library path="hoverboard_hardware_interface">
+  <class name="hoverboard_hardware_interface/HoverboardHardwareInterface"
+         type="hoverboard_hardware_interface::HoverboardHardwareInterface"
+         base_class_type="hardware_interface::SystemInterface">
+    <description>
+        The hoverboard motors hardware interface for ros2_control
+    </description>
+  </class>
+</library>
\ No newline at end of file
diff --git a/hoverboard_ros2_control/hoverboard_hardware_interface/include/hoverboard_hardware_interface/hoverboard_hardware_interface.hpp b/hoverboard_ros2_control/hoverboard_hardware_interface/include/hoverboard_hardware_interface/hoverboard_hardware_interface.hpp
new file mode 100644
index 0000000..a62fd53
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_hardware_interface/include/hoverboard_hardware_interface/hoverboard_hardware_interface.hpp
@@ -0,0 +1,83 @@
+// Copyright 2023 Robert Gruberski (Viola Robotics Sp. z o.o. Poland)
+//
+// 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.
+
+#pragma once
+
+#include <cstddef>
+
+#include "rclcpp/rclcpp.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+#include "serial_port_service.hpp"
+#include "motor_wheel.hpp"
+
+namespace hoverboard_hardware_interface
+{
+    class HoverboardHardwareInterface : public hardware_interface::SystemInterface
+    {
+        struct HardwareConfig
+        {
+            std::string leftWheelJointName = "left_wheel_joint";
+            std::string rightWheelJointName = "right_wheel_joint";
+
+            float loopRate = 30.0;
+            int encoderTicksPerRevolution = 1024;
+        };
+
+        struct SerialPortConfig
+        {
+            std::string device = "/dev/ttyUSB0";
+            int baudRate = 115200;
+            int timeout = 1000;
+        };
+
+    public:
+        RCLCPP_SHARED_PTR_DEFINITIONS(HoverboardHardwareInterface)
+
+        hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo &) override;
+
+        hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
+
+        hardware_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override;
+
+        hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;
+
+        hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override;
+
+        std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+        std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+        hardware_interface::return_type read(const rclcpp::Time &, const rclcpp::Duration &) override;
+
+        hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override;
+
+        void motorWheelFeedbackCallback(MotorWheelFeedback);
+
+    private:
+
+        SerialPortService serialPortService;
+
+        HardwareConfig hardwareConfig;
+        SerialPortConfig serialPortConfig;
+
+        MotorWheel leftWheel;
+        MotorWheel rightWheel;
+
+        bool connect();
+        bool disconnect();
+    };
+}
\ No newline at end of file
diff --git a/hoverboard_ros2_control/hoverboard_hardware_interface/include/hoverboard_hardware_interface/motor_wheel.hpp b/hoverboard_ros2_control/hoverboard_hardware_interface/include/hoverboard_hardware_interface/motor_wheel.hpp
new file mode 100644
index 0000000..d14ba8a
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_hardware_interface/include/hoverboard_hardware_interface/motor_wheel.hpp
@@ -0,0 +1,79 @@
+// Copyright 2023 Robert Gruberski (Viola Robotics Sp. z o.o. Poland)
+//
+// 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.
+
+#pragma once
+
+#include <string>
+#include <cmath>
+
+#define ENCODER_MIN_VALUE 0
+#define ENCODER_MAX_VALUE 9000
+#define ENCODER_LOW_WRAP_FACTOR 0.3
+#define ENCODER_HIGH_WRAP_FACTOR 0.7
+
+namespace hoverboard_hardware_interface
+{
+    class MotorWheel {
+    public:
+        std::string name = "";
+
+        int64_t encoderTicks = 0;
+        int16_t encoderTicksPrevious = 0;
+        int16_t encoderOverflowCount = 0;
+
+        int16_t encoderLowWrap;
+        int16_t encoderHighWrap;
+
+        double command = 0.0;
+        double position = 0.0;
+        double velocity = 0.0;
+
+        double radiansPerRevolution = 0.0;
+
+        MotorWheel() = default;
+
+        MotorWheel(const std::string &wheelJointName, int encoderTicksPerRevolution)
+        {
+            name = wheelJointName;
+            radiansPerRevolution = ((2 * M_PI) / encoderTicksPerRevolution);
+
+            encoderLowWrap = ENCODER_LOW_WRAP_FACTOR * (ENCODER_MAX_VALUE - ENCODER_MIN_VALUE) + ENCODER_MIN_VALUE;
+            encoderHighWrap = ENCODER_HIGH_WRAP_FACTOR * (ENCODER_MAX_VALUE - ENCODER_MIN_VALUE) + ENCODER_MIN_VALUE;
+        }
+
+        double calculateEncoderAngle()
+        {
+            return encoderTicks * radiansPerRevolution;
+        }
+
+        void updateEncoderTicks(int16_t newTicks)
+        {
+            if(newTicks < encoderLowWrap && encoderTicksPrevious > encoderHighWrap)
+            {
+                encoderOverflowCount++;
+            }
+
+            if(newTicks > encoderHighWrap && encoderTicksPrevious < encoderLowWrap)
+            {
+                encoderOverflowCount--;
+            }
+
+            encoderTicks = (encoderOverflowCount * ENCODER_MAX_VALUE) + newTicks;
+            encoderTicksPrevious = newTicks;
+        }
+
+    private:
+
+    };
+}
diff --git a/hoverboard_ros2_control/hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_protocol.hpp b/hoverboard_ros2_control/hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_protocol.hpp
new file mode 100644
index 0000000..0227cdd
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_protocol.hpp
@@ -0,0 +1,47 @@
+// Copyright 2023 Robert Gruberski (Viola Robotics Sp. z o.o. Poland)
+//
+// 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.
+
+#pragma once
+
+#include <cstdint>
+
+#define HEAD_FRAME 0xABCD
+
+// enum class MOTOR_STATES {
+//     UNOCUPPIED = 0b00,
+//     RUN = 0b01,
+//     BRAKE = 0b11,
+//     LOCK_SHAFT = 0b10,
+// };
+
+typedef struct {
+    uint16_t head;
+    int16_t  command1;
+    int16_t  command2;
+    int16_t  rightMotorSpeed;
+    int16_t  leftMotorSpeed;
+    int16_t  rightMotorEncoderCumulativeCount;
+    int16_t  leftMotorEncoderCumulativeCount;
+    int16_t  batteryVoltage;
+    int16_t  boardTemperature;
+    uint16_t commandLed;
+    uint16_t checksum;
+} MotorWheelFeedback;
+
+typedef struct {
+    uint16_t head = HEAD_FRAME;
+    int16_t  steer;
+    int16_t  speed;
+    uint16_t checksum;
+} MotorWheelDriveControl;
diff --git a/hoverboard_ros2_control/hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_service.hpp b/hoverboard_ros2_control/hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_service.hpp
new file mode 100644
index 0000000..dad4631
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_service.hpp
@@ -0,0 +1,69 @@
+// Copyright 2023 Robert Gruberski (Viola Robotics Sp. z o.o. Poland)
+//
+// 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.
+
+#pragma once
+
+#include <string>
+
+#include <boost/asio.hpp>
+#include <boost/thread.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "serial_port_protocol.hpp"
+
+#define SERIAL_PORT_READ_BUF_SIZE 256
+
+typedef boost::shared_ptr<boost::asio::serial_port> serial_port_ptr;
+
+namespace hoverboard_hardware_interface
+{
+    class SerialPortService
+    {
+        public:
+
+        SerialPortService() = default;
+
+        bool connect(const std::string &serial_device, int baud_rate, int timeout);
+        bool disconnect();
+
+        void read();
+        void asyncRead();
+
+        int write(const char *, const int &);
+
+        void BindMotorWheelFeedbackCallback(std::function<void(MotorWheelFeedback)>);
+
+        private:
+
+        boost::asio::io_service io_service;
+        serial_port_ptr port;
+        boost::mutex mutex;
+
+        uint16_t head_frame = 0;
+        uint16_t msg_counter = 0;
+        uint8_t msg_command = 0;
+
+        char prev_byte = 0;
+        char* p{};
+
+        char read_buf_raw[SERIAL_PORT_READ_BUF_SIZE]{};
+
+        void onReceive(const boost::system::error_code&, size_t);
+
+        std::function<void(MotorWheelFeedback)> motorWheelFeedbackCallback;
+
+        MotorWheelFeedback motorWheelFeedback {};
+    };
+}
diff --git a/hoverboard_ros2_control/hoverboard_hardware_interface/package.xml b/hoverboard_ros2_control/hoverboard_hardware_interface/package.xml
new file mode 100644
index 0000000..79ebd71
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_hardware_interface/package.xml
@@ -0,0 +1,23 @@
+<?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>hoverboard_hardware_interface</name>
+  <version>0.1.0</version>
+  <description>ROS Control hardware interface implementation for Hoverboard Motors</description>
+  <maintainer email="rgruberski@gmail.com">Robert Gruberski</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>libboost-system-dev</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/hoverboard_ros2_control/hoverboard_hardware_interface/src/hoverboard_hardware_interface.cpp b/hoverboard_ros2_control/hoverboard_hardware_interface/src/hoverboard_hardware_interface.cpp
new file mode 100644
index 0000000..c8bd650
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_hardware_interface/src/hoverboard_hardware_interface.cpp
@@ -0,0 +1,201 @@
+// Copyright 2023 Robert Gruberski (Viola Robotics Sp. z o.o. Poland)
+//
+// 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 "hoverboard_hardware_interface/hoverboard_hardware_interface.hpp"
+
+namespace hoverboard_hardware_interface
+{
+    hardware_interface::CallbackReturn HoverboardHardwareInterface::on_init(const hardware_interface::HardwareInfo & info)
+    {
+        if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
+            return hardware_interface::CallbackReturn::ERROR;
+        }
+
+        hardwareConfig.leftWheelJointName = info.hardware_parameters.at("left_wheel_joint_name");
+        hardwareConfig.rightWheelJointName = info.hardware_parameters.at("right_wheel_joint_name");
+        hardwareConfig.loopRate = std::stof(info.hardware_parameters.at("loop_rate"));
+        // hardwareConfig.encoderTicksPerRevolution = std::stoi(info.hardware_parameters.at("encoder_ticks_per_revolution"));
+
+        serialPortConfig.device = info.hardware_parameters.at("device");
+        serialPortConfig.baudRate = std::stoi(info.hardware_parameters.at("baud_rate"));
+        serialPortConfig.timeout = std::stoi(info.hardware_parameters.at("timeout"));
+
+        leftWheel = MotorWheel(info.hardware_parameters.at("left_wheel_joint_name"), 
+                                std::stoi(info.hardware_parameters.at("encoder_ticks_per_revolution")));
+        rightWheel = MotorWheel(info.hardware_parameters.at("right_wheel_joint_name"), 
+                                std::stoi(info.hardware_parameters.at("encoder_ticks_per_revolution")));
+
+        for (const hardware_interface::ComponentInfo & joint : info.joints)
+        {
+            if (joint.command_interfaces.size() != 1)
+            {
+                RCLCPP_FATAL(
+                    rclcpp::get_logger("HoverboardHardwareInterface"),
+                    "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), joint.command_interfaces.size());
+
+                return hardware_interface::CallbackReturn::ERROR;
+            }
+
+            if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY)
+            {
+                RCLCPP_FATAL(
+                    rclcpp::get_logger("HoverboardHardwareInterface"),
+                    "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
+                    joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
+                
+                return hardware_interface::CallbackReturn::ERROR;
+            }
+
+            if (joint.state_interfaces.size() != 2)
+            {
+                RCLCPP_FATAL(
+                    rclcpp::get_logger("HoverboardHardwareInterface"),
+                    "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), joint.state_interfaces.size());
+                
+                return hardware_interface::CallbackReturn::ERROR;
+            }
+
+            if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
+            {
+                RCLCPP_FATAL(
+                    rclcpp::get_logger("HoverboardHardwareInterface"),
+                    "Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(),
+                    joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
+                
+                return hardware_interface::CallbackReturn::ERROR;
+            }
+
+            if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY)
+            {
+                RCLCPP_FATAL(
+                    rclcpp::get_logger("HoverboardHardwareInterface"),
+                    "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(),
+                    joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY);
+                
+                return hardware_interface::CallbackReturn::ERROR;
+            }
+        }
+
+        return hardware_interface::CallbackReturn::SUCCESS;
+    }
+
+    std::vector<hardware_interface::StateInterface> HoverboardHardwareInterface::export_state_interfaces()
+    {
+        std::vector<hardware_interface::StateInterface> state_interfaces;
+
+        state_interfaces.emplace_back(hardware_interface::StateInterface(leftWheel.name, hardware_interface::HW_IF_POSITION, &leftWheel.position));
+        state_interfaces.emplace_back(hardware_interface::StateInterface(leftWheel.name, hardware_interface::HW_IF_VELOCITY, &leftWheel.velocity));
+
+        state_interfaces.emplace_back(hardware_interface::StateInterface(rightWheel.name, hardware_interface::HW_IF_POSITION, &rightWheel.position));
+        state_interfaces.emplace_back(hardware_interface::StateInterface(rightWheel.name, hardware_interface::HW_IF_VELOCITY, &rightWheel.velocity));
+
+        return state_interfaces;
+    }
+
+    std::vector<hardware_interface::CommandInterface> HoverboardHardwareInterface::export_command_interfaces()
+    {
+        std::vector<hardware_interface::CommandInterface> command_interfaces;
+
+        command_interfaces.emplace_back(hardware_interface::CommandInterface(leftWheel.name, hardware_interface::HW_IF_VELOCITY, &leftWheel.command));
+        command_interfaces.emplace_back(hardware_interface::CommandInterface(rightWheel.name, hardware_interface::HW_IF_VELOCITY, &rightWheel.command));
+
+        return command_interfaces;
+    }
+
+    hardware_interface::CallbackReturn HoverboardHardwareInterface::on_configure(const rclcpp_lifecycle::State &)
+    {
+        RCLCPP_INFO(rclcpp::get_logger("HoverboardHardwareInterface"), "Configuring... please wait a moment...");
+
+        if (!serialPortService.connect(serialPortConfig.device, serialPortConfig.baudRate, serialPortConfig.timeout))
+        {
+            return hardware_interface::CallbackReturn::ERROR;
+        }
+
+        serialPortService.BindMotorWheelFeedbackCallback(
+            std::bind(&HoverboardHardwareInterface::motorWheelFeedbackCallback, this, std::placeholders::_1)
+        );
+
+        return hardware_interface::CallbackReturn::SUCCESS;
+    }
+
+    hardware_interface::CallbackReturn HoverboardHardwareInterface::on_cleanup(const rclcpp_lifecycle::State &)
+    {
+        RCLCPP_INFO(rclcpp::get_logger("HoverboardHardwareInterface"), "Cleaning up... please wait a moment...");
+
+        if (!serialPortService.disconnect())
+        {
+            return hardware_interface::CallbackReturn::ERROR;
+        }
+
+        return hardware_interface::CallbackReturn::SUCCESS;
+    }
+
+    hardware_interface::CallbackReturn HoverboardHardwareInterface::on_activate(const rclcpp_lifecycle::State &)
+    {
+        // TODO: add some logic
+        RCLCPP_INFO(rclcpp::get_logger("HoverboardHardwareInterface"), "Activating... please wait a moment...");
+
+        return hardware_interface::CallbackReturn::SUCCESS;
+    }
+
+    hardware_interface::CallbackReturn HoverboardHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &)
+    {
+        // TODO: add some logic
+        RCLCPP_INFO(rclcpp::get_logger("HoverboardHardwareInterface"), "Deactivating... please wait a moment...");
+
+        return hardware_interface::CallbackReturn::SUCCESS;
+    }
+
+    hardware_interface::return_type HoverboardHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration & period)
+    {
+        serialPortService.read();
+
+        double lastPosition = leftWheel.position;
+        leftWheel.position = leftWheel.calculateEncoderAngle();
+        leftWheel.velocity = (leftWheel.position - lastPosition) / period.seconds();
+
+        lastPosition = rightWheel.position;
+        rightWheel.position = rightWheel.calculateEncoderAngle();
+        rightWheel.velocity = (rightWheel.position - lastPosition) / period.seconds();
+
+        return hardware_interface::return_type::OK;
+    }
+
+    hardware_interface::return_type HoverboardHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &)
+    {
+        MotorWheelDriveControl motorWheelDriveControl;
+
+        const double speed = ((leftWheel.command / 0.10472) + (rightWheel.command / 0.10472)) / 2.0;
+        const double steer = ((leftWheel.command / 0.10472) - speed) * 2.0;
+
+        // TODO: radius should be read from the urdf file, check calculations
+        motorWheelDriveControl.speed = (int16_t) (speed);
+        motorWheelDriveControl.steer = (int16_t) (steer);
+        motorWheelDriveControl.checksum = (uint16_t)(motorWheelDriveControl.head ^ motorWheelDriveControl.steer ^ motorWheelDriveControl.speed);
+
+        // RCLCPP_INFO(rclcpp::get_logger("SerialPortService"), "%i %i", motorWheelDriveControl.speed, motorWheelDriveControl.steer);
+
+        serialPortService.write((const char*) &motorWheelDriveControl, sizeof(MotorWheelDriveControl));
+
+        return hardware_interface::return_type::OK;
+    }
+
+    void HoverboardHardwareInterface::motorWheelFeedbackCallback(MotorWheelFeedback motorWheelFeedback) 
+    {
+        leftWheel.updateEncoderTicks(motorWheelFeedback.leftMotorEncoderCumulativeCount);
+        rightWheel.updateEncoderTicks(motorWheelFeedback.rightMotorEncoderCumulativeCount);
+    }
+}
+
+PLUGINLIB_EXPORT_CLASS(hoverboard_hardware_interface::HoverboardHardwareInterface, hardware_interface::SystemInterface)
diff --git a/hoverboard_ros2_control/hoverboard_hardware_interface/src/serial_port_service.cpp b/hoverboard_ros2_control/hoverboard_hardware_interface/src/serial_port_service.cpp
new file mode 100644
index 0000000..4d03558
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_hardware_interface/src/serial_port_service.cpp
@@ -0,0 +1,145 @@
+// Copyright 2023 Robert Gruberski (Viola Robotics Sp. z o.o. Poland)
+//
+// 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 "hoverboard_hardware_interface/serial_port_service.hpp"
+
+using namespace hoverboard_hardware_interface;
+
+bool SerialPortService::connect(const std::string &serial_device, int baud_rate, int timeout)
+{
+    boost::system::error_code ec;
+
+    if (port) {
+        RCLCPP_ERROR(rclcpp::get_logger("SerialPortService"), "Port is already opened...");
+        return false;
+    }
+
+    port = serial_port_ptr(new boost::asio::serial_port(io_service));
+    port->open(serial_device, ec);
+
+    if (ec) {
+        RCLCPP_ERROR(rclcpp::get_logger("SerialPortService"), "Connection to the %s failed..., error: %s",
+            serial_device, ec.message().c_str());
+        return false;
+    }
+
+    port->set_option(boost::asio::serial_port_base::baud_rate(baud_rate));
+    port->set_option(boost::asio::serial_port_base::character_size(8));
+    port->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
+    port->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
+    port->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
+
+    // TODO: try to run it asynchronously
+    // boost::thread t([ObjectPtr = &io_service] { return ObjectPtr->run(); });
+    // t.detach();
+
+    return true;
+}
+
+bool SerialPortService::disconnect()
+{
+    boost::mutex::scoped_lock look(mutex);
+
+    if (port) {
+        port->cancel();
+        port->close();
+        port.reset();
+    }
+
+    io_service.stop();
+    io_service.reset();
+
+    return true;
+}
+
+void SerialPortService::read()
+{
+    boost::mutex::scoped_lock look(mutex);
+
+    size_t bytes_transferred = port->read_some(boost::asio::buffer(read_buf_raw, SERIAL_PORT_READ_BUF_SIZE));
+    
+    for (unsigned int i = 0; i < bytes_transferred; ++i) {
+
+        head_frame = ((uint16_t)(read_buf_raw[i]) << 8) | (uint8_t) prev_byte;
+
+        if (head_frame == HEAD_FRAME)
+        {
+            p = (char*) &motorWheelFeedback;
+            *p++ = prev_byte;
+            *p++ = read_buf_raw[i];
+            msg_counter = 2;
+        }
+        else if (msg_counter >= 2 && msg_counter < sizeof(MotorWheelFeedback)) {
+            *p++ = read_buf_raw[i];
+            msg_counter++;
+        }
+
+        if (msg_counter == sizeof(MotorWheelFeedback))
+        {
+            motorWheelFeedbackCallback(motorWheelFeedback);
+
+            msg_counter = 0;
+        }
+
+        prev_byte = read_buf_raw[i];
+    }
+}
+
+void SerialPortService::asyncRead()
+{
+    if (port.get() == nullptr || !port->is_open()) {
+        RCLCPP_ERROR(rclcpp::get_logger("SerialPortService"), "Port is already closed...");
+        return;
+    }
+
+    port->async_read_some(
+            boost::asio::buffer(read_buf_raw, SERIAL_PORT_READ_BUF_SIZE),
+            boost::bind(&SerialPortService::onReceive,
+                    this, boost::asio::placeholders::error,
+                    boost::asio::placeholders::bytes_transferred));
+}
+
+void SerialPortService::onReceive(const boost::system::error_code& ec, size_t bytes_transferred)
+{
+    RCLCPP_INFO(rclcpp::get_logger("SerialPortService"), "onReceive async event...");
+
+    // boost::mutex::scoped_lock look(mutex);
+
+    // if (port.get() == nullptr || !port->is_open()) return;
+
+    // if (ec) {
+    //     asyncRead();
+    //     return;
+    // }
+}
+
+int SerialPortService::write(const char * message, const int & size)
+{
+    boost::system::error_code ec;
+
+    if (port.get() == nullptr || !port->is_open()) {
+        RCLCPP_ERROR(rclcpp::get_logger("SerialPortService"), "Port is already closed...");
+        return 0;
+    }
+
+    if (size == 0) {
+        return 0;
+    }
+
+    return port->write_some(boost::asio::buffer(message, size), ec);
+}
+
+void SerialPortService::BindMotorWheelFeedbackCallback(std::function<void(MotorWheelFeedback)> fn) {
+    motorWheelFeedbackCallback = fn;
+}
\ No newline at end of file
diff --git a/hoverboard_ros2_control/hoverboard_ros2_control/CMakeLists.txt b/hoverboard_ros2_control/hoverboard_ros2_control/CMakeLists.txt
new file mode 100644
index 0000000..418c0bd
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_ros2_control/CMakeLists.txt
@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(hoverboard_ros2_control)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()
diff --git a/hoverboard_ros2_control/hoverboard_ros2_control/package.xml b/hoverboard_ros2_control/hoverboard_ros2_control/package.xml
new file mode 100644
index 0000000..9788a38
--- /dev/null
+++ b/hoverboard_ros2_control/hoverboard_ros2_control/package.xml
@@ -0,0 +1,18 @@
+<?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>hoverboard_ros2_control</name>
+  <version>0.1.0</version>
+  <description>ROS Control package for Hoverboard Motors</description>
+  <maintainer email="rgruberski@gmail.com">Robert Gruberski</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/robot_base_description/config/hoverboard_controllers.yaml b/robot_base_description/config/hoverboard_controllers.yaml
index a2856a6..a756e84 100644
--- a/robot_base_description/config/hoverboard_controllers.yaml
+++ b/robot_base_description/config/hoverboard_controllers.yaml
@@ -1,6 +1,6 @@
 controller_manager:
   ros__parameters:
-    update_rate: 50  # Hz
+    update_rate: 200  # Hz
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
@@ -10,8 +10,8 @@ controller_manager:
 
 hoverboard_base_controller:
   ros__parameters:
-    left_wheel_names: ["left_front_wheel_joint"]
-    right_wheel_names: ["right_front_wheel_joint" ]
+    left_wheel_names: ["left_front_wheel_joint", "left_back_wheel_joint"]
+    right_wheel_names: ["right_front_wheel_joint", "right_back_wheel_joint"]
 
     wheel_separation: 0.44
     wheels_per_side: 1  
@@ -21,7 +21,7 @@ hoverboard_base_controller:
     left_wheel_radius_multiplier: 1.0
     right_wheel_radius_multiplier: 1.0
 
-    publish_rate: 100.0
+    publish_rate: 200.0
     odom_frame_id: odom
     base_frame_id: base_link
     pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
@@ -30,7 +30,7 @@ hoverboard_base_controller:
     open_loop: false
     enable_odom_tf: true
 
-    cmd_vel_timeout: 0.5
+    cmd_vel_timeout: 0.1
     #publish_limited_velocity: true
     use_stamped_vel: false
     #velocity_rolling_window_size: 10
diff --git a/robot_base_description/urdf/robot_base.xacro b/robot_base_description/urdf/robot_base.xacro
index bb7b467..9e308e6 100644
--- a/robot_base_description/urdf/robot_base.xacro
+++ b/robot_base_description/urdf/robot_base.xacro
@@ -7,6 +7,20 @@
 <xacro:include filename="$(find robot_base_description)/urdf/ros2_control.xacro" />
 <xacro:include filename="$(find robot_base_description)/urdf/robot_base.gazebo" /> 
 
+<!-- Define the front and back serial devices 
+  <xacro:property name="front_device" value="/dev/ttyUSB0"/> 
+  <xacro:property name="back_device" value="/dev/ttyUSB1"/>  -->
+
+
+<!-- Include the hoverboard_ros2_control macro with front and back devices 
+  <xacro:macro name="include_hoverboard_ros2_control" params="front_device back_device">
+  <xacro:hoverboard_ros2_control name="hoverboard_ros2_control" front_device="${front_device}" back_device="${back_device}"/>
+  </xacro:macro>    
+
+ <xacro:include_hoverboard_ros2_control front_device="${front_device}" back_device="${back_device}"/> 
+  
+  -->
+
 <link name="base_link">
   
   <visual>
@@ -1655,5 +1669,6 @@
 </joint>
 
 <xacro:hoverboard_ros2_control name="Hoverboard" />
+<!-- <xacro:include_hoverboard_ros2_control front_device="${front_device}" back_device="${back_device}"/> -->
 
 </robot>
diff --git a/robot_base_description/urdf/ros2_control.xacro b/robot_base_description/urdf/ros2_control.xacro
index 7a0cb65..3a982f1 100644
--- a/robot_base_description/urdf/ros2_control.xacro
+++ b/robot_base_description/urdf/ros2_control.xacro
@@ -8,8 +8,11 @@
         <plugin>hoverboard_hardware_interface/HoverboardHardwareInterface</plugin>
         <param name="left_front_wheel_joint_name">left_front_wheel_joint</param>
         <param name="right_front_wheel_joint_name">right_front_wheel_joint</param>
-        <param name="loop_rate">50</param>
-        <param name="device">/dev/ttyUSB0</param>
+        <param name="left_back_wheel_joint_name">left_back_wheel_joint</param>
+        <param name="right_back_wheel_joint_name">right_back_wheel_joint</param>
+        <param name="loop_rate">100</param>
+        <param name="front_device">/dev/ttyUSB0</param>
+        <param name="back_device">/dev/ttyUSB1</param>
         <param name="baud_rate">115200</param>
         <param name="timeout">1000</param>
         <param name="encoder_ticks_per_revolution">90</param>
@@ -26,11 +29,22 @@
         <state_interface name="position"/>
         <state_interface name="velocity"/>
       </joint>
-      
 
+      <joint name="left_back_wheel_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
       
+      <joint name="right_back_wheel_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
+
     </ros2_control>
 
   </xacro:macro>
 
 </robot>
+