diff --git a/setup.py b/setup.py index c7d910e..d8897b6 100644 --- a/setup.py +++ b/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' ], }, ) diff --git a/wave_rover_controller/joy_control_node b/wave_rover_controller/joy_control_node new file mode 100644 index 0000000..ab909ad --- /dev/null +++ b/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() diff --git a/wave_rover_controller/wave_rover_controller.py b/wave_rover_controller/wave_rover_controller.py new file mode 100644 index 0000000..0e20d6f --- /dev/null +++ b/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