diff --git a/serial_motor_demo/config/robot_params.yaml b/serial_motor_demo/config/robot_params.yaml
new file mode 100644
index 0000000..e048698
--- /dev/null
+++ b/serial_motor_demo/config/robot_params.yaml
@@ -0,0 +1,11 @@
+# robot_params.yaml
+wheel_odometry_node:
+  ros__parameters:
+    wheel_radius: 0.058  # Meters
+    wheel_base: 0.284     # Meters
+    # Add other parameters as needed
+
+motor_command_node:
+  ros__parameters:
+    wheel_radius: 0.058  # Meters
+    wheel_base: 0.284     # Meters
diff --git a/serial_motor_demo/launch/motor_driver.launch.py b/serial_motor_demo/launch/motor_driver.launch.py
new file mode 100644
index 0000000..77136ec
--- /dev/null
+++ b/serial_motor_demo/launch/motor_driver.launch.py
@@ -0,0 +1,65 @@
+# my_launch_file.launch.py
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+
+    config = os.path.join(
+        get_package_share_directory('serial_motor_demo'),
+        'config',
+        'robot_params.yaml'
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'speed', default_value='0.3',
+            description='Speed for teleop_twist_keyboard'),
+        DeclareLaunchArgument(
+            'turn', default_value='0.5',
+            description='Turn for teleop_twist_keyboard'),
+        DeclareLaunchArgument(
+            'serial_port', default_value='/dev/ttyUSB0'),
+
+        
+        Node(
+            package='serial_motor_demo',
+            executable='driver',
+            name='driver',
+            output='screen',
+            parameters=[
+                {'serial_port': LaunchConfiguration('serial_port')},
+            ],
+        ),
+        Node(
+            package='teleop_twist_keyboard',
+            executable='teleop_twist_keyboard',
+            name='teleop_twist_keyboard',
+            output='screen',
+            prefix='xterm -e',
+            parameters=[
+                {'speed': LaunchConfiguration('speed')},
+                {'turn': LaunchConfiguration('turn')},
+            ],
+        ),
+        Node(
+            package='serial_motor_demo',
+            executable='motor_command_node',
+            name='motor_command_node',
+            output='screen',
+            parameters=[config],
+            
+        ),
+
+        Node(
+            package='serial_motor_demo',
+            executable='wheels_odometry',
+            name='wheel_odometry_node',
+            output='screen',
+            parameters=[config],
+        ),
+    ])
diff --git a/serial_motor_demo/motor_driver_launch.png b/serial_motor_demo/motor_driver_launch.png
new file mode 100644
index 0000000..3e552b0
Binary files /dev/null and b/serial_motor_demo/motor_driver_launch.png differ
diff --git a/serial_motor_demo/package.xml b/serial_motor_demo/package.xml
index 49b03ae..01fbb10 100644
--- a/serial_motor_demo/package.xml
+++ b/serial_motor_demo/package.xml
@@ -11,6 +11,9 @@
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>
   <test_depend>python3-pytest</test_depend>
+  
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
 
   <depend>rclpy</depend>
   <depend>serial_motor_demo_msgs</depend>
diff --git a/serial_motor_demo/serial_motor_demo/driver.py b/serial_motor_demo/serial_motor_demo/driver.py
index 6b2018a..dc2421c 100644
--- a/serial_motor_demo/serial_motor_demo/driver.py
+++ b/serial_motor_demo/serial_motor_demo/driver.py
@@ -18,17 +18,17 @@ def __init__(self):
 
         # Setup parameters
 
-        self.declare_parameter('encoder_cpr', value=0)
+        self.declare_parameter('encoder_cpr', value=17200)
         if (self.get_parameter('encoder_cpr').value == 0):
             print("WARNING! ENCODER CPR SET TO 0!!")
 
 
-        self.declare_parameter('loop_rate', value=0)
+        self.declare_parameter('loop_rate', value=30)
         if (self.get_parameter('loop_rate').value == 0):
             print("WARNING! LOOP RATE SET TO 0!!")
 
 
-        self.declare_parameter('serial_port', value="/dev/ttyUSB0")
+        self.declare_parameter('serial_port', value="/dev/ttyACM0")
         self.serial_port = self.get_parameter('serial_port').value
 
 
diff --git a/serial_motor_demo/serial_motor_demo/motor_command_node.py b/serial_motor_demo/serial_motor_demo/motor_command_node.py
new file mode 100644
index 0000000..b0759fa
--- /dev/null
+++ b/serial_motor_demo/serial_motor_demo/motor_command_node.py
@@ -0,0 +1,69 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from serial_motor_demo_msgs.msg import MotorCommand
+import math
+
+class MotorCommandNode(Node):
+    def __init__(self):
+        super().__init__('motor_command_node')
+
+        # Create a subscriber to listen to Twist messages
+        self.create_subscription(
+            Twist, 
+            'cmd_vel',  # Topic name might be different based on your setup
+            self.twist_callback, 
+            10
+        )
+        
+        # Create a publisher to send out MotorCommand messages
+        self.publisher = self.create_publisher(MotorCommand, 'motor_command', 10)
+
+        # Especificaciones del robot
+        self.declare_parameter('wheel_radius', 0.0553, ignore_override=True)
+        self.declare_parameter('wheel_base', 0.284, ignore_override=True)
+
+        # Retrieve parameter values
+        self.wheel_radius = self.get_parameter('wheel_radius').value
+        self.wheel_base = self.get_parameter('wheel_base').value
+
+        
+        self.get_logger().info('Motor Command Node Started')
+        
+    def twist_callback(self, msg):
+        try:
+            # Assuming linear.x controls the speed and angular.z controls the steering
+            speed = msg.linear.x
+            rotation = msg.angular.z
+
+            
+
+            mot_1_speed = (speed + (rotation * self.wheel_base / 2)) / self.wheel_radius
+            mot_2_speed = (speed - (rotation * self.wheel_base / 2)) / self.wheel_radius
+
+            # Convert to rad/s
+            mot_1_speed = mot_1_speed / (2 * math.pi)
+            mot_2_speed = mot_2_speed / (2 * math.pi)
+            
+            # Create and publish the motor command
+            motor_msg = MotorCommand()
+            motor_msg.is_pwm = False
+            motor_msg.mot_1_req_rad_sec = mot_1_speed
+            motor_msg.mot_2_req_rad_sec = mot_2_speed
+            self.publisher.publish(motor_msg)
+
+            self.get_logger().info(f'Motor Commands sent: mot_1 = {mot_1_speed:.2f} rad/s, mot_2 = {mot_2_speed:.2f} rad/s')
+
+        except Exception as e:
+            self.get_logger().error(f'Error in twist_callback: {str(e)}')
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = MotorCommandNode()
+    rclpy.spin(node)
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
diff --git a/serial_motor_demo/serial_motor_demo/wheels_odometry.py b/serial_motor_demo/serial_motor_demo/wheels_odometry.py
new file mode 100644
index 0000000..cf49c47
--- /dev/null
+++ b/serial_motor_demo/serial_motor_demo/wheels_odometry.py
@@ -0,0 +1,87 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist, Pose, Quaternion, Point, TransformStamped
+from nav_msgs.msg import Odometry
+from utils.transformations import euler2quaternion
+from math import sin, cos
+import tf2_ros
+
+from serial_motor_demo_msgs.msg import MotorVels
+
+
+class OdometryNode(Node):
+    def __init__(self):
+        super().__init__('wheel_odometry_node')
+        self.subscription = self.create_subscription(
+            MotorVels, 'motor_vels', self.listener_callback, 10)
+        self.publisher = self.create_publisher(Odometry, 'wheel_odom', 10)
+        self.odom_broadcaster = tf2_ros.TransformBroadcaster(self)
+        self.last_time = self.get_clock().now()
+
+        # Estado inicial del robot
+        self.x = 0.0
+        self.y = 0.0
+        self.theta = 0.0
+
+        # Especificaciones del robot
+        self.wheel_radius = self.declare_parameter('wheel_radius', 0.0553).value
+        self.wheel_base = self.declare_parameter('wheel_base', 0.284).value
+
+    def listener_callback(self, msg):
+        current_time = self.get_clock().now()
+        dt = (current_time - self.last_time).nanoseconds / 1e9
+        self.last_time = current_time
+
+        # Calcular velocidades lineales de las ruedas
+        v_left = msg.mot_1_rad_sec * self.wheel_radius
+        v_right = msg.mot_2_rad_sec * self.wheel_radius
+
+        # Calcular la velocidad lineal y angular del robot
+        V = (v_right + v_left) / 2
+        omega = (v_right - v_left) / self.wheel_base
+
+        # Actualizar la posición y orientación
+        self.x += V * cos(self.theta) * dt
+        self.y += V * sin(self.theta) * dt
+        self.theta += omega * dt
+
+        # Crear y llenar el mensaje de odometría
+        odom = Odometry()
+        odom.header.stamp = current_time.to_msg()
+        odom.header.frame_id = "odom"
+        odom.child_frame_id = "base_link"
+        orientation=euler2quaternion(0, 0, self.theta)
+        odom.pose.pose = Pose(
+            position=Point(x=self.x, y=self.y, z=0.0),
+            orientation=Quaternion(x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3])
+        )
+            
+        # Aquí puedes agregar la velocidad (Twist) si es necesario
+
+        self.publisher.publish(odom)
+
+        # Enviar transformación
+        t = TransformStamped()
+        t.header.stamp = current_time.to_msg()
+        t.header.frame_id = 'odom'
+        t.child_frame_id = 'base_link'
+        t.transform.translation.x = self.x
+        t.transform.translation.y = self.y
+        quaternion = euler2quaternion(0, 0, self.theta)
+        # Use the quaternion for rotation
+        t.transform.rotation.x = quaternion[0]
+        t.transform.rotation.y = quaternion[1]
+        t.transform.rotation.z = quaternion[2]
+        t.transform.rotation.w = quaternion[3]
+
+        self.odom_broadcaster.sendTransform(t)
+
+def main(args=None):
+    rclpy.init(args=args)
+    odometry_node = OdometryNode()
+    rclpy.spin(odometry_node)
+    odometry_node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
diff --git a/serial_motor_demo/setup.cfg b/serial_motor_demo/setup.cfg
index bd0b17c..eafb207 100644
--- a/serial_motor_demo/setup.cfg
+++ b/serial_motor_demo/setup.cfg
@@ -1,4 +1,4 @@
 [develop]
-script-dir=$base/lib/serial_motor_demo
+script_dir=$base/lib/serial_motor_demo
 [install]
-install-scripts=$base/lib/serial_motor_demo
+install_scripts=$base/lib/serial_motor_demo
diff --git a/serial_motor_demo/setup.py b/serial_motor_demo/setup.py
index d7e142e..5691f03 100644
--- a/serial_motor_demo/setup.py
+++ b/serial_motor_demo/setup.py
@@ -1,4 +1,6 @@
 from setuptools import setup
+import os
+from glob import glob
 
 package_name = 'serial_motor_demo'
 
@@ -6,10 +8,14 @@
     name=package_name,
     version='0.0.0',
     packages=[package_name],
+    py_modules=['utils.transformations'],
     data_files=[
         ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
+         ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        # Include launch files
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -19,9 +25,12 @@
     license='TODO: License declaration',
     tests_require=['pytest'],
     entry_points={
-        'console_scripts': [
-            'gui = serial_motor_demo.gui:main',
-            'driver = serial_motor_demo.driver:main'
-        ],
-    },
+    'console_scripts': [
+        'gui = serial_motor_demo.gui:main',
+        'driver = serial_motor_demo.driver:main',
+        'motor_command_node = serial_motor_demo.motor_command_node:main',
+        'wheels_odometry = serial_motor_demo.wheels_odometry:main',
+    ],
+},
+
 )
diff --git a/serial_motor_demo/utils/__init__.py b/serial_motor_demo/utils/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/serial_motor_demo/utils/transformations.py b/serial_motor_demo/utils/transformations.py
new file mode 100644
index 0000000..542a289
--- /dev/null
+++ b/serial_motor_demo/utils/transformations.py
@@ -0,0 +1,179 @@
+""" 
+$$$$$$$\  $$$$$$$$\  $$$$$$\   $$$$$$\  $$$$$$$\  $$$$$$$$\       $$$$$$$\    $$\   
+$$  __$$\ $$  _____|$$  __$$\ $$  __$$\ $$  __$$\ $$  _____|      $$  __$$\ $$$$ |  
+$$ |  $$ |$$ |      $$ /  \__|$$ /  $$ |$$ |  $$ |$$ |            $$ |  $$ |\_$$ |  
+$$$$$$$  |$$$$$\    $$ |      $$ |  $$ |$$$$$$$  |$$$$$\          $$$$$$$  |  $$ |  
+$$  ____/ $$  __|   $$ |      $$ |  $$ |$$  __$$< $$  __|         $$  ____/   $$ |  
+$$ |      $$ |      $$ |  $$\ $$ |  $$ |$$ |  $$ |$$ |            $$ |        $$ |  
+$$ |      $$$$$$$$\ \$$$$$$  | $$$$$$  |$$ |  $$ |$$$$$$$$\       $$ |      $$$$$$\ 
+\__|      \________| \______/  \______/ \__|  \__|\________|      \__|      \______|
+"""
+# * PECORE - Master en Automática y Control en Robótica               *
+# * Universitat Politècnica de Catalunya (UPC)                         *
+# *                                                                    *
+# * Participantes:                                                     *
+# * - Victor Escribano Garcia                                          *
+# * - Alejandro Acosta Montilla                                       *
+# *                                                                    *
+# * Año: 2023   
+# 
+# * FIND THE ORIGINAL CODE IN THE FOLLOWING GITHUB REPO: https://github.com/VictorEscribano/PECORE/tree/main                                                                                  
+"""
+Descripcion: Funciones para poder transformar entre matrices de transformacion y TransformStamped de ROS. 
+"""                                                                        
+
+from geometry_msgs.msg import TransformStamped, PoseStamped
+import numpy as np
+import math
+
+
+# Define a function to calculate the transformation matrix
+def Rt2homo_matrix(translation, rotation):
+    # Reconstruct the transformation matrix
+    # Create a 4x4 identity matrix
+    matrix = np.identity(4)
+
+    # Set the translation components in the matrix
+    matrix[0, 3] = translation.x
+    matrix[1, 3] = translation.y
+    matrix[2, 3] = translation.z
+
+    # Extract the rotation components
+    x = rotation.x
+    y = rotation.y
+    z = rotation.z
+    w = rotation.w
+
+    # Calculate the rotation matrix components
+    xx = x * x
+    xy = x * y
+    xz = x * z
+    yy = y * y
+    yz = y * z
+    zz = z * z
+    wx = w * x
+    wy = w * y
+    wz = w * z
+
+    # Set the rotation components in the matrix
+    matrix[0, 0] = 1 - 2 * (yy + zz)
+    matrix[0, 1] = 2 * (xy - wz)
+    matrix[0, 2] = 2 * (xz + wy)
+
+    matrix[1, 0] = 2 * (xy + wz)
+    matrix[1, 1] = 1 - 2 * (xx + zz)
+    matrix[1, 2] = 2 * (yz - wx)
+
+    matrix[2, 0] = 2 * (xz - wy)
+    matrix[2, 1] = 2 * (yz + wx)
+    matrix[2, 2] = 1 - 2 * (xx + yy)
+
+    return matrix
+
+
+def homo_matrix2tf(matrix, parent_frame, child_frame, clock_stamp):
+    transform_stamped = TransformStamped()
+    transform_stamped.header.stamp = clock_stamp
+    transform_stamped.header.frame_id = parent_frame
+    transform_stamped.child_frame_id = child_frame
+
+    # Extract translation from the matrix
+    translation = np.array([matrix[0, 3], matrix[1, 3], matrix[2, 3]])
+
+    # Extract rotation (quaternion) from the matrix
+    trace = matrix[0, 0] + matrix[1, 1] + matrix[2, 2]
+    if trace > 0:
+        s = 0.5 / np.sqrt(trace + 1.0)
+        w = 0.25 / s
+        x = (matrix[2, 1] - matrix[1, 2]) * s
+        y = (matrix[0, 2] - matrix[2, 0]) * s
+        z = (matrix[1, 0] - matrix[0, 1]) * s
+    elif matrix[0, 0] > matrix[1, 1] and matrix[0, 0] > matrix[2, 2]:
+        s = 2.0 * np.sqrt(1.0 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2])
+        w = (matrix[2, 1] - matrix[1, 2]) / s
+        x = 0.25 * s
+        y = (matrix[0, 1] + matrix[1, 0]) / s
+        z = (matrix[0, 2] + matrix[2, 0]) / s
+    elif matrix[1, 1] > matrix[2, 2]:
+        s = 2.0 * np.sqrt(1.0 + matrix[1, 1] - matrix[0, 0] - matrix[2, 2])
+        w = (matrix[0, 2] - matrix[2, 0]) / s
+        x = (matrix[0, 1] + matrix[1, 0]) / s
+        y = 0.25 * s
+        z = (matrix[1, 2] + matrix[2, 1]) / s
+    else:
+        s = 2.0 * np.sqrt(1.0 + matrix[2, 2] - matrix[0, 0] - matrix[1, 1])
+        w = (matrix[1, 0] - matrix[0, 1]) / s
+        x = (matrix[0, 2] + matrix[2, 0]) / s
+        y = (matrix[1, 2] + matrix[2, 1]) / s
+        z = 0.25 * s
+
+    # Set the translation and rotation in the TransformStamped message
+    transform_stamped.transform.translation.x = translation[0]
+    transform_stamped.transform.translation.y = translation[1]
+    transform_stamped.transform.translation.z = translation[2]
+    transform_stamped.transform.rotation.x = x
+    transform_stamped.transform.rotation.y = y
+    transform_stamped.transform.rotation.z = z
+    transform_stamped.transform.rotation.w = w
+
+    return transform_stamped
+
+def transform2pose(transform):
+    pose = PoseStamped()
+    pose.header = transform.header
+    pose.pose.position.x = transform.transform.translation.x
+    pose.pose.position.y = transform.transform.translation.y
+    pose.pose.position.z = transform.transform.translation.z
+    pose.pose.orientation = transform.transform.rotation
+    return pose
+
+def euler2quaternion(ai, aj, ak):
+    ai /= 2.0
+    aj /= 2.0
+    ak /= 2.0
+    ci = math.cos(ai)
+    si = math.sin(ai)
+    cj = math.cos(aj)
+    sj = math.sin(aj)
+    ck = math.cos(ak)
+    sk = math.sin(ak)
+    cc = ci*ck
+    cs = ci*sk
+    sc = si*ck
+    ss = si*sk
+    q = np.empty((4, ))
+    q[0] = cj*sc - sj*cs
+    q[1] = cj*ss + sj*cc
+    q[2] = cj*cs - sj*sc
+    q[3] = cj*cc + sj*ss
+    return q
+
+
+def quaternion2euler(x, y, z, w):
+        """
+        Convert a quaternion into euler angles (roll, pitch, yaw)
+        roll is rotation around x in radians (counterclockwise)
+        pitch is rotation around y in radians (counterclockwise)
+        yaw is rotation around z in radians (counterclockwise)
+        """
+        t0 = +2.0 * (w * x + y * z)
+        t1 = +1.0 - 2.0 * (x * x + y * y)
+        roll_x = math.atan2(t0, t1)
+     
+        t2 = +2.0 * (w * y - z * x)
+        t2 = +1.0 if t2 > +1.0 else t2
+        t2 = -1.0 if t2 < -1.0 else t2
+        pitch_y = math.asin(t2)
+     
+        t3 = +2.0 * (w * z + x * y)
+        t4 = +1.0 - 2.0 * (y * y + z * z)
+        yaw_z = math.atan2(t3, t4)
+     
+        return roll_x, pitch_y, yaw_z # in radians
+
+def normalize_angle(angle):
+    while angle > math.pi:
+        angle -= 2.0 * math.pi
+    while angle < -math.pi:
+        angle += 2.0 * math.pi
+    return angle
\ No newline at end of file