Implement teleop Keyboard

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

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],
+        ),
+    ])

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>

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
 
 

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()

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()

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

serial_motor_demo/setup.py

@@ -1,15 +1,21 @@
 from setuptools import setup
+import os
+from glob import glob
 
 package_name = 'serial_motor_demo'
 
 setup(
     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',
+    ],
+},
+
 )