added controller anf joy one

setup.py

@@ -1,4 +1,6 @@
 from setuptools import find_packages, setup
+import os
+from glob import glob
 
 package_name = 'wave_rover_controller'
 
@@ -10,6 +12,8 @@ setup(
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), [f for f in glob('launch/*') if os.path.isfile(f)]) ,
+        (os.path.join('share', package_name, 'config'), [f for f in glob('config/*') if os.path.isfile(f)]) ,
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -24,6 +28,7 @@ setup(
     },
     entry_points={
         'console_scripts': [
+            'joy_control_node = wave_rover_controller.joy_control_node:main'
         ],
     },
 )

wave_rover_controller/joy_control_node

@@ -0,0 +1,17 @@
+#!/usr/bin/env python3
+
+import rclpy
+from wave_rover_controller.robot_controller.robot_controller import WaveRoverController
+from unity_robot_controller.joy_control_node import JoyControlNode
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = JoyControlNode(WaveRoverController)
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

wave_rover_controller/wave_rover_controller.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from wave_rover_controller.robot_controller.robot_controller import RobotController
+from serial import Serial
+from serial.exceptions import SerialException
+import json
+
+class WaveRoverController(RobotController):
+
+    def __init__(self, node):
+        super().__init__()
+
+        self._node = node
+
+
+        self._serial_port = self.declare_parameter('serial_port', '/dev/ttyUSB0').value
+        self._baudrate = self.declare_parameter('baudrate', 115200).value
+
+        wheel_max = 0.5 # popugais
+        self._linear_max = wheel_max
+        self._base = 0.2 # m
+        self._angular_max = 2 * wheel_max / self._base #
+
+
+        # Serial подключение
+        self._serial_conn: Optional[Serial] = None
+        self._connect_serial()
+
+    def _connect_serial(self):
+        try:
+            self._serial_conn = Serial(
+                port=self._serial_port, baudrate=self._baudrate, timeout=1.0
+            )
+            time.sleep(0.5)
+            self._node.get_logger().info(f'Подключено к {self._serial_port}')
+        except SerialException as e:
+            self._node.get_logger().error(f'Ошибка serial: {e}')
+            self._serial_conn = None
+
+
+    def _send_json_command(self, command: dict):
+        if not self._serial_conn or not self._serial_conn.is_open:
+            self._connect_serial()
+            return
+
+        try:
+            json_str = json.dumps(command)
+            self._serial_conn.write(json_str.encode() + b'\n')
+            self._node.get_logger().debug(f'Отправлено: {json_str}')
+        except SerialException as e:
+            self._node.get_logger().error(f'Ошибка отправки: {e}')
+            self._serial_conn.close()
+
+
+    def send_speed_cmd(self, v, w):
+        v, w = super(WaveRoverController, self).send_speed_cmd(v, w)
+
+        vp = v * self._linear_max
+        wp = w * self._angular_max
+
+        left_speed, right_speed = self._twist_to_wheel_speeds(vp, wp)
+
+        command = {"T": 1, "L": left_speed, "R": right_speed}
+        self._send_json_command(command)
+
+
+
+    def _twist_to_wheel_speeds(self, linear_x: float, angular_z: float):
+        """Twist -> скорости колес для дифференциального привода"""
+        wheel_base = 0.2  # м
+
+        left_speed = (linear_x - (wheel_base * angular_z) / 2) / self.max_linear_speed
+        right_speed = (linear_x + (wheel_base * angular_z) / 2) / self.max_linear_speed
+
+        left_speed = max(-1.0, min(1.0, left_speed))
+        right_speed = max(-1.0, min(1.0, right_speed))
+
+        return left_speed, right_speed