Feat: first draw of the driver

Author: robotcopper

package.xml

@@ -0,0 +1,17 @@
+<?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>ros2_lx16a_driver</name>
+  <version>0.0.0</version>
+  <description>Package ROS2 for controlling LewanSoul LX-16A servos</description>
+  <maintainer email="[email protected]">florisjousselin</maintainer>
+  <license>BSD3</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>serial</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

resource/ros2_lx16a_driver

@@ -0,0 +1,29 @@
+import serial
+import struct
+
+class LX16ADriver:
+    def __init__(self, port, baudrate=115200):
+        self.ser = serial.Serial(port, baudrate, timeout=1)
+
+    def send_command(self, servo_id, command, params):
+        length = 3 + len(params)
+        packet = [0x55, 0x55, servo_id, length, command] + params
+        checksum = ~sum(packet[2:]) & 0xFF
+        packet.append(checksum)
+        self.ser.write(bytearray(packet))
+
+    def move(self, servo_id, position):
+        pos = int(position * 1000 / 240)
+        params = list(struct.pack('<H', pos))
+        self.send_command(servo_id, 1, params)
+
+    def get_physical_angle(self, servo_id):
+        self.send_command(servo_id, 28, [])
+        response = self.ser.read(7)
+        if len(response) == 7:
+            angle = struct.unpack('<H', response[5:7])[0]
+            return angle * 240 / 1000
+        return None
+
+    def close(self):
+        self.ser.close()

ros2_lx16a_driver/__init__.py

@@ -0,0 +1,32 @@
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float64
+from .lx16a_driver import LX16ADriver
+
+class LX16ANode(Node):
+    def __init__(self):
+        super().__init__('lx16a_node')
+        self.driver = LX16ADriver('/dev/ttyUSB0')  # Adapter au port série correct
+        self.subscription = self.create_subscription(
+            Float64,
+            'servo_angle',
+            self.set_servo_angle,
+            10
+        )
+        self.get_logger().info('LX16A Node started')
+
+    def set_servo_angle(self, msg):
+        angle = msg.data
+        self.get_logger().info(f'Setting servo to {angle} degrees')
+        self.driver.move(1, angle)
+
+    def destroy_node(self):
+        self.driver.close()
+        super().destroy_node()
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = LX16ANode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()

ros2_lx16a_driver/lx16a_driver.py

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/ros2_lx16a_driver
+[install]
+install_scripts=$base/lib/ros2_lx16a_driver

ros2_lx16a_driver/lx16a_node.py

@@ -0,0 +1,25 @@
+from setuptools import setup
+
+package_name = 'ros2_lx16a_driver'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='florisjousselin',
+    maintainer_email='[email protected]',
+    description='Package ROS2 for controlling LewanSoul LX-16A servos',
+    license='BSD3',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'lx16a_node = ros2_lx16a_driver.lx16a_node:main',
+        ],
+    },
+)

setup.cfg

@@ -0,0 +1,8 @@
+import unittest
+from ros2_lx16a.lx16a_driver import LX16ADriver
+
+class TestLX16ADriver(unittest.TestCase):
+    def test_move(self):
+        driver = LX16ADriver('/dev/null')  
+        driver.move(1, 90)
+        self.assertTrue(True)